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A  control  system  for  a  single  link  light-weight  flexible  robot  manipulator 
is  designed  and  implemented  on  a  Digital  Signal  Processing  (DSP)  chip,  which 
controls  the  link  dynamics  via  the  applied  motor  torque.  Different  models  of  this 
flexible  structure  are  studied  including  a  nonlinear  Galerkin  model,  linearized 
Galerkin  model,  and  linear  beam  theory  model;  and  these  are  compared  through 
simulation  to  the  empirical  system  response.  A  geometric  Input-Output  Lin¬ 
earization  of  the  nonlinear  system  is  achieved  with  respect  to  the  hub  angle  and 
hub  rate.  Experimental  system  identification,  performed  on  the  flexible  beam, 
suggests  we  can  adequately  model  the  flexible  beam  using  linear  theory  and  an 
optimization  of  the  Feedback  Controller  is  performed.  Finally,  this  controller 
is  implemented  on  a  DSP  chip  and  various  aspects  (programming,  timing,  syn¬ 
chronization,  etc.)  of  DSP-based  feedback  control  system  implementation  are 
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CHAPTER 

ONE 


Introduction 


This  thesis  considers  the  problem  of  controlling  a  light-weight  single  link 
robot  manipulator  arm  which  sacrifices  rigidity  of  the  link  for  reduced  mass 
and  bulk.  The  increased  flexibility  in  the  robot  manipulator  link  causes  the 
resonant  frequencies  of  the  link  to  shift  to  lower  frequencies  that  create  unde¬ 
sirable  vibrational  effects  in  a  robot  manipulator.  The  goal  of  this  thesis  is  to 
design  a  feedback  control  system  which  provides  the  robot  arm  with  the  ability 
to  move  in  a  swift  and  controlled  manner  while  minimizing  these  vibrations.  If 
the  goal  were  to  merely  servo  the  base  of  the  arm  as  fast  as  possible,  without 
any  regard  to  the  flexible  structure  attached,  the  resonant  modes  of  the  beam 
would  be  excited.  Depending  on  the  amount  of  damping,  it  could  then  take  a 
very  long  time  for  the  tip  position  to  settle  to  the  reference  position.  Without 
a  controlled  system  which  takes  into  account  the  flexibility  in  the  beam,  we  can 
not  accurately  position  the  endpoint  of  the  robot  arm  in  a  reasonable  amount 
of  time. 

Analysis  of  light-weight  flexible  structures  is  necessary  if  we  are  to  under¬ 
stand  the  dynamics  of  space  platforms  which  have  controlled  flexible  appendages. 
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A  controlled  flexible  structure  could  be  a  long  and  slender  robot  arm  (with  in¬ 
herent  flexibility)  working  in  the  NASA  Space  Shuttle  bay  or  possibly  an  an¬ 
tenna  system  which  is  attached  to  a  satellite  undergoing  a  change  of  orientation. 
In  both  cases,  we  have  a  flexible  structure  extending  radially  from  a  rigid  body 
which  is  subject  to  a  driving  force  and/or  torque.  The  only  difference  is  whether, 
for  control  purposes,  we  are  interested  primarily  in  positioning  the  rigid  body 
itself  or  the  distal  end  of  the  flexible  structure.  Furthermore,  control  of  such 
structures  continues  to  be  an  active  area  of  research  in  the  literature. 

Before  we  are  able  to  control  a  flexible  link  manipulator,  we  need  to  deter¬ 
mine  the  characteristics  of  the  system  and  we  will  see  that  the  resonant  fre¬ 
quencies  only  partially  describe  the  dynamics  involved.  The  beam  dynamics  are 
described  by  the  Euler  Bernoulli  beam  equations,  a  partial  differential  equation 
whose  terms  depend  on  time  and  the  distance  along  the  beam.  Thus  the  first 
task  at  hand  is  to  identify  the  characteristics  of  the  system  and  explicitly  choose 
a  suitable  model.  The  models  describing  the  dynamics  that  we  will  explore  are 
simple  linear  models  of  finite  dimension  and  a  nonlinear  model  which  arises  from 
a  spatial  discretization  (finite  element  approximation)  of  the  distributed  param¬ 
eter  system.  These  models  will  be  simulated  and  compared  to  the  experimental 
system  response  before  designing  the  control  system  with  the  most  appropriate 
model  of  the  system.  With  these  chosen  models,  we  will  empirically  estimate 
or  explicitly  determine  the  model  parameters  so  we  can  simulate  and  finally 
initiate  a  design  of  the  feedback  control  system.  The  goal  of  the  control  system 
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design  is  to  optimize  the  step  response  of  the  tip  position  to  a  given  change  in 
the  reference  command. 

The  last  section  describes  the  implementation  of  the  control  system  which  is 
achieved  through  use  of  a  Digital  Signal  Processing  (DSP)  chip  which  has  vastly 
improved  processing  power  when  compared  to  past  experiments  of  this  type  [1] 
[2]  using  the  80286  processor  on  the  IBM  PC  AT.  Additionally,  this  thesis  serves 
as  an  example  of  how  to  implement  a  general  sampled  data  feedback  control 
system  on  a  high  speed  DSP  chip  and  discusses  some  of  the  signal  processing 
issues  inherent  in  such  a  system. 
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CHAPTER 

TWO 


Description  of  Experiment  and  Hardware 


2.1  Hardware  Description  of  the  Flexible  Beam  Experiment 

The  experimental  hardware  in  figure  2.1  consists  of  a  single-link  robot  manip¬ 
ulator  with  link  flexibility,  an  IBM  PC  AT  computer  with  DSP  board,  three 
sensors  and  some  additional  electronics.  The  flexible  link  robot  consists  of  a 
brushless  DC  torque  motor,  optical  encoder  collocated  to  measure  shaft  posi¬ 
tion,  tachometer  to  measure  shaft  rate  and  a  non-collocated  accelerometer  placed 
at  the  distal  end  of  the  flexible  beam.  The  flexible  beam  is  a  one  meter  length  of 
aluminum  which  is  mounted  to  the  hub  of  the  motor  and  serves  to  model  a  full 
size  flexible  robot  arm  or  space  craft  antennae  system.  Throughout  this  thesis, 
we  will  view  the  experiment  as  modelling  a  robot  arm  although  the  hardware 
could  be  thought  to  model  other  dynamical  systems. 

The  electronics  which  we  have  installed  to  measure,  process,  and  control  the 
flexible  robot  consist  of: 

1.  IBM  PC  AT  Computer 

2.  Communication,  Automation,  and  Control  Inc.  DSP  board  with  AT&T 
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7.  Power  Amplifier 


Although  we  do  not  include  here  a  tip  position  sensor,  these  electronics 
comprise  the  digital  control  system  hardware  which,  as  we  will  later  see,  will 
control  the  tip  position  of  the  flexible  link  manipulator  through  use  of  a  state 
observer.  This  tip  position  state  is  available  for  feedback  so  this  experiment  is 
related  to  that  of  Schmitz  [3]  which  made  use  of  an  optical  tip  position  sensor 
for  feedback. 

The  DDA06  board  has  in  addition  to  the  D/A  output  function  to  the  motor, 
three  digital  I/O  ports  (with  word  size  of  8);  two  of  which  are  used  to  input  the 
12  bit  word  from  the  optical  shaft  encoder  to  the  computer.'* 

2.2  Digital  Controller  Processor 

The  DSP  board  by  Communications,  Automation,  &  Control  Inc.  is  a  low 
cost  plug  in  board  which  uses  AT&T’s  DSP32  floating  point  processor  and  also 
'  has  I/O  consisting  of  serial  and  parallel  ports  as  well  as  a  Codec  telephone 
sampler.  This  DSP  chip  peaks  at  8  MFLOPS.  The  DSP  chip  has  on-board 
memory  of  4  Kbytes  and  56  Kbytes  additional  off  chip  memory  which  is  directly 
addressable  and  located  in  four  memory  chips  adjacent  to  the  DSP  chip  on  the 
board.  For  a  more  detailed  description  of  the  DSP  chip  as  well  as  examples  of 
DSP  programming,  see  chapter  5  and  the  appendix. 

The  DSP  chip  communicates  to  the  IBM  PC  AT  via  the  parallel  port  of 
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Figure  2.2:  Bus  Diagram 


the  chip  through  the  PC  bus.  This  is  a  16  bit  bus  with  a  clock  rate  of  6  Mhz. 
Also  located  on  the  PC  bus  is  the  ADC  and  DAC  boards  which  communicate  to 
the  IBM  PC  AT.  The  tachometer  and  accelerometer  sensors  are  sampled  by  the 
DASH16  ADC  board  operating  under  computer  control  by  the  IBM  processor. 
Timing  and  synchronization  are  controlled  by  the  IBM  processor  which  uses  one 
of  the  clocks  on  the  DASH16  board. 


2.3  Sampling  Rate  Selection  and  Prefilter  Design 


2.3.1  Sampling  Rate  Selection 

The  sampling  rate  of  the  digital  controller  must  be  adequately  high  to  accurately 
sense  the  majority  of  signals  that  are  present  in  our  system.  A  rule  of  thumb 
[4]  which  gives  good  performance  is  to  have  approximately  twenty  samples  for 
the  highest  mode  that  is  present  in  the  system,  ie.  the  desired  sampling  rate  fa 
is  determined  by  the  equation 

fs  =  20  x  fmax.  (2.3.1) 

Since  our  system  is  an  infinite  dimensional  system,  we  must  assume  that, 

i  a 

all  modes  above  a  given  frequency  are  negligible  and  therefore  assume  a  fixed 
number  of  flexible  modes.  As  we  will  see  in  the  next  chapter,  the  first  three 
modes  or  vibration  frequencies  of  this  system  are  the  dominant  ones  and  they 
are  approximately  at  9 Hz,  19 Hz,  and  49 Hz.  To  fully  model  all  these  modes 
would  require  an  eigth  order  linear  model  since  the  motor  accounts  for  a  second 
order  system.  Since  we  want  the  order  of  our  observer  to  be  of  reasonable  size, 
we  will  try  to  limit  the  order  of  our  modelled  system  to  six  by  neglecting  the 
mode  at  49 Hz.  This  is  reasonable  for  our  hardware  but  on  the  other  hand,  if 
we  were  to  try  to  model  the  mode  at  49 Hz,  according  to  the  above  rule,  we 
would  need  to  sample  the  sensor  data  at  20  x  49  ^  IK  Hz.  Additionally,  the 
DSP  processor  would  be  required  to  compute  the  estimate  of  eight  states  in  less 
time.  We  could  build  this  eigth  order  controller  but  the  goal  here  was  to  build 
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a  more  practical  low  order  controller. 

If  we  attempt  to  model  our  system  as  a  sixth  order  model,  we  will  be  able 
to  capture  the  dynamics  of  the  motor  as  well  as  retain  the  first  two  modes 
of  the  flexible  beam.  Since  the  second  flexible  mode  is  approximately  20  Hz, 
we  would  thus  like  to  sample  at  400  Hz,  and  the  feedback  controller  will  be 
designed  and  implemented  accordingly.  In  addition  to  this  chosen  sampling 
rate,  another  version  of  the  digital  controller  will  be  designed  and  implemented 
with  a  sampling  rate  of  200  Hz  to  determine  if  there  is  any  degradation  in  the 
performance  at  the  lower  sampling  rate. 


2.3.2  Prefilter  Design 


To  prevent  the  aliasing  of  the  sampled  sensor  data,  analog  prefilters  were  placed 
between  the  two  analog  sensor  outputs  and  the  A/D  input  board.  A  second 
order  low  pass  butterworth  filter  ((  =  .71)  with  transfer  function 


G,  M 


LU~ 


s2  +  2  (us  +  u)2 


(2.3.2) 


was  realized  with  a  LC  1458  dual  op  amp,  some  resistors  and  capacitors  (see 
Table  2.1  )  to  achieve  the  desired  cut  off  frequency  of  the  filter.  The  bandwidth 
of  the  analog  prefilter  was  selected  according  to  the  following  rule  [4]  which 
depends  on  the  sampling  rate  h. 


uch  ~  0.5  to  1 


(2.3.3) 
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Figure  2.3:  Prefilter  Circuit 


Table  2.1:  Prefilter  Circuit  Components 

According  to  the  above  rule,  with  a  sampling  rate  of  either  200Hz  or  400Hz  , 
the  cutoff  frequency  fc  =  40  would  be  adequate  but  we  select  the  cutoff  frequency 

fc  =  27.2  Hz.  (2.3.4) 

to  provide  more  attenuation  of  the  mode  at  49  Hz  while  retaining  the  first  two 
modes  at  8.2  and  19  Hz.  The  circuit  for  the  analog  prefilter  is  shown  if  figure 
2.3  with  the  component  values  given  in  table  2.1. 


CHAPTER 

_ THREE 

Modeling  and  System  Identification 


The  flexible  link  manipulator  is  modeled  as  a  flexible  beam  with  a  rigid  body 
attached  at  the  base  end  which  represents  the  hub  and  motor  and  the  input  to 
the  system  is  the  motor  torque.  The  Euler  Bernoulli  beam  equations  governing 
the  flexible  beam  are  arrived  at  through  Hamilton’s  principle.  Finally,  the  open 
loop  transfer  functions  are  found  by  taking  the  Laplace  transform  of  the  Euler 
Bernoulli  beam  equations.  We  will  then  recognize  some  important  properties  of 
these  transfer  functions  which  will  aid  us  in  the  experimental  determination  of 
the  transfer  function  parameters.  The  nonminimum  phase  properity  as  well  as 
the  pole  zero  alternation  properity  will  give  us  some  additional  constraints  on 
the  numerator  and  denominator  polynomials  of  our  transfer  function  estimate. 
Our  estimate  of  the  transfer  function  will  be  obtained  by  finding  the  parameters, 
under  these  constraints,  which  best  matches  both  the  magnitude  and  phase  of 
the  transfer  function  estimate  Bode  plot  to  that  of  the  empirically  determined 
frequency  response. 
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3.1  Hamilton’s  Principle 


At  any  instant  in  time,  the  flexible  beam  system  is  described  by  the  state  of  its  n 
generalized  coordinate  vectors  within  the  configuration  space.  As  time  goes  on, 
the  state  of  the  system  changes  and  this  state  traces  a  curve  which  is  known  as 
the  path  of  the  system.  The  virtual  displacement  8  is  defined  as  an  infinitesimal 
and  arbitrary  change  in  the  actual  motion  or  path  of  a  system  which  satisfies 
any  constraints  that  are  placed  on  the  motion  between  times  t\  and  t2. 

Unlike  the  symbol  d  which  designates  differentials,  the  virtual  displacement 
has  no  time  change  associated  with  it,  but  the  operations  concerning  8  obey, 
those  of  elementary  calculus. 

The  Lagrangian  C  is  the  difference  between  the  kinetic  energy  and  potential 
energies 

C  —  T  —  V. 


The  motion  of  the  system  from  time  to  time  U  is  such  that  the  line  integral 


/' 

Ju 


Cdt 


(3.1.1) 


has  a  stationary  value  for  the  correct  or  actual  path  of  motion  so  Hamilton’s 
principle  is  summarized  by  saying  that  the  motion  is  such  that  the  variation  of 
the  line  integral  3.1.1  for  fixed  1 1  and  t2  is  zero: 


ft  2 

8  Cdt  =  0 
Jt  i 


(3.1.2) 


Hamilton’s  principle  is  invariant  with  respect  to  coordinate  changes,  and  the 
conditions  which  render  the  integral  stationary  lead  to  the  equations  of  motion 
of  the  system. 


3.2  Euler  Bernoulli  Beam  Equation 


The  Euler  Bernoulli  Beam  equations  which  describe  the  dynamics  of  the  beam 
are  derived  using  Hamilton’s  Principle  and  are  outlined  here.  For  the  full  deriva¬ 
tion  see  [5]. 

The  total  kinetic  energy  of  the  hub,  beam  of  length  L,  and  tip  mass  mt  is 
given  by 


Tk  =  IH02  + 


r< 


dw(x,t) 

dt 


+  xd)2pdx  +  mt( 


diu(x,t) 

dt 


+  xd)2\x=L  (3.2.3) 


where  is  the  hub  inertia  and  w(x,  t )  is  a  function  of  length  along  the  beam 
x,  at  time  t  which  represents  the  perpendicular  distance  of  the  beam  from  a 
reference  line  fixed  to  the  hub.  The  strain  energy  is 


V. 


__  1  [L 
=  2  Jo 


El 


'd2w(x,t)' 
dx 2 


dx 


(3.2  A) 


where  E  is  Young’s  Modulus  and  /  is  the  second  moment  of  area  of  the  beam  s 
cross  section.  The  motor  torque  is  an  external  torque  and  is 


Ve,t  =  -T9 


where  T  is  the  input  torque.  The  Lagrangian  C  for  this  system  is 


C  =  Tk-Vs-  Vext 
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and  if  we  apply  Hamilton’s  Principle  ,  i.e,  set, 


S  f\Tk  -Vs-  Vext)dt  =  0 
Jtx 


(3.2.5) 


we  can  derive  the  the  partial  differential  equations  describing  the  beam  dynam¬ 
ics.  Let  p  be  a  constant  mass  density  of  the  beam  material  and  after  some 
manipulations  using  integration  by  parts  we  can  arrive  at  the  Euler  Bernoulli 


beam  equation 


ei^A =  -pxo 


dx4 


6t 2 


with  the  boundary  conditions 


(3.2.6) 


dx 2 


=  0 


w(0,t ) 

„rd2w(x,t)l 
EI—W~'*=L 
d3w . 


=  0 


El 


dx 3 


\x=L 


,d2iu 

™<( W  +  xd'\*=L- 


(3.2.7) 

(3.2.8) 

(3.2.9) 
(3.2.10) 


If  we  define 


y(x,  t)  =  w(x,  t)  4-  x9(t) 


the  equations  can  be  recast  into  the  form 


EJd4y{x,t)  +  &y(xxt) 


dx4 


dt2 


=  0 


(3.2.11) 


with  the  boundary  conditions 


El 


d2y(x,t) 
dx 2 


,=o  +  T  —  IuQ  —  0 


(3.2.12) 
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(3.2.13) 


y(o,f) 


d2y(x,t ) 

33y, 


FI 


5a:3 


|ar=L 


o 


mt 


&Vt 

dt2 


\x=L  ■ 


(3.2.14) 

(3.2.15) 


If  we  allow  a  beam  with  nonuniform  mass  distribution  m{x)  or  nonuniform 
inertia  /(a:),  then  the  Euler  Bernoulli  equation  becomes 

52 

dx 2 


EI(x) 


d2y(x,t) I 


dx2 


=  —m(x) 


d  y(y,t) 
dt 2 


(3.2.16) 


3.3  Open  Loop  Transfer  Functions 


The  open  loop  transfer  functions  for  the  flexible  beam  and  rotating  hub  system 
are  found  by  taking  the  Laplace  transform  of  the  Euler  Bernoulli  beam  equations 
solving  the  equations  as  in  [6]  [5]  [3].  We  then  define  complex  number  A  which 
is  related  to  s  by  the  equations 


PA 

A 


_ P_ 

El 

PL 


s 


2 


If  Y(x,  s )  is  the  Laplace  transform  of  y(x1t),  then  the  solution  to  the  system  is 
given  by 

Y(x,  s)  —  A  sin  Px  +  B  sinh  px  +  C  cos  Px  +  D  cosh  Px 

We  seek  transfer  functions  for  the  three  sensors  we  have  as  well  as  the  tip  position 
so  we  can  estimate  the  tip  position.  The  exact  open  loop  transfer  functions  ,as 
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derived  by  Schmitz  [3]  for  a  similar  experiment,  are  given  by  the  expressions 


B(s) 

dY(0  ,s) 
dx 

Hub  Angle 

(3.3.17) 

Q(s) 

0Y(O,s) 

5  dx 

Hub  Angular  Rate 

(3.3.18) 

=  s2Y(L,s ) 

Tip  Acceleration 

(3.3.19) 

Vtip(s) 

=  Y(L,s) 

Tip  Position 

(3.3.20) 

and  we  can  define  following  numerator  polynomials  N ()  and  denominator  poly¬ 
nomials  N()  for  the  transfer  functions 

e(s) 

O(s) 

Cbip('S) 

ynp(s) 

3.3.1  Hub  Position  Transfer  Function 


N0(s) 

Na(s) 

Natip(s) 

Nytip(s) 


D(s)-] 


(3.3.21} 


The  hub  position  transfer  function  is  of  the  form 

*(<>)  i  + 

n>)  ^tld+S) 

i 

where  the  total  inertia  of  the  system  It  is  given  by 

It  =  Ib  +  Ih  +  mtL~. 


(3.3.22) 


This  transfer  function  has  an  interesting  property  that  between  any  two 
successive  roots  of  the  numerator  No  there  is  always  a  unique  root  of  the  de¬ 
nominator.  Thus  the  poles  and  zeroes  alternate  along  the  jui  axis  and  we  will 
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call  this  phenomenon  the  pole  zero  alternation  property.  Also  it  is  apparent 
that  at  low  frequencies  the  beam  behaves  as  This  fact  will  help  us  to  esti¬ 
mate  the  transfer  function  from  the  empirically  determined  frequency  response 
using  prior  knowledge  on  the  theory  of  beams. 

Constraining  our  transfer  function  to  a  sixth  order  model  and  observing  the 
pole  zero  alternation  property,  we  still  need  to  choose  the  parameters  of  the 
transfer  function 

k  A  +  ^  +  *) 

/lS(s  +  a)'=‘  (5  +  2&i  +  l) 

where  w,_i  <  ft,-  <  a for  i  =  1,2  and  £  >  0. 


3.3.2  Tip  Position  Transfer  Function 


For  the  tip  position,  the  numerator  of  the  transfer  function  has  as  its  zeros  the 
roots  of  the  equation  [3] 

NytW  =  J^2^smX  +  sinh  A) 


and  the  transfer  function  is  given  by 

t  .=00(1-4) 

(3.3.24) 

For  this  transfer  function  we  know  that  the  zeroes  are  all  on  the  real  axis 


yjM  _  L  ‘Sf  U-  sj) 

A*)  h*2  fi  (1  + 


having  symmetric  pairs  about  the  origin.  Since  there  exists  zeroes  in  the  positive 
right  half  plane,  this  transfer  function  is  nonminimum  phase. 

A  second  property  of  this  transfer  function  is  that  the  zeros  don  not  depend 
on  the  tip  mass  since  they  correspond  to  input  frequencies  in  which  the  tip  does 
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not  move.  A  mass  place  at  the  tip  while  responding  to  the  sinusoidal  input  at. 


the  frequency  of  the  zeroes  will  have  no  effect  on  the  system. 

Again,  by  constraining  the  transfer  function  to  a  sixth  order  model  and  ob¬ 
serving  the  pole  zero  alternation  property,  we  still  need  to  choose  the  parameters 
of  the  transfer  function 

k  d(w~2^  +  1 

Its(s  +  a)l\  ?^  +  2£a  +  1 
where  <  Qt-  <  w,-  for  i  =  1,2  and  (  >  0. 

3.3.3  Nonminimum  Phase  Property 

Because  of  the  nonminimum  phase  property  of  the  flexible  beam,  we  know  that  if 
the  system  is  driven  with  a  step  input,  that  the  tip  response  can  first  move  in  the 
opposite  direction  to  the  hub  response  after  an  initial  delay  of  quasi  stationary 
motion  [7].  The  amount  of  delay  in  the  system  depends  on  the  placement  of 
the  zero  which  is  closest  to  the  origin.  Furthermore,  we  should  expect  the  tip 
accelerometer  to  first  move  in  the  negative  direction  and  this  is  precisely  what 
was  experimentally  observed. 

3.4  Spatial  Discretization  -  Galerkin  Model 

The  Euler  Bernoulli  Beam  equation  is  a  partial  differential  equation  which  is 
function  of  time  and  position  on  the  beam  and  is  thus  a  distributed  parameter 
system.  One  method  for  solving  the  distributed  parameter  system  is  to  approxi- 
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mate  the  continuous  beam  by  a  finite  number  of  rigid  beams  or  elements,  which 
are  adjoined  by  freely  rotating  pin  joints  connected  in  a  lengthwise  manner.  At 
each  joint  is  also  connected  a  spring  to  provide  a  linear  restoring  force  and  a 
damping  element  can  be  included  in  each  joint  as  well.  Posbergh  [6]  has  shown 
that  as  the  number  of  elements  in  this  approximation  approaches  infinity,  the 
solution  converges  to  that  of  the  continuous  model. 

If  we  write  the  equations  for  this  finite  dimensional  system,  called  a  Galerkin 
model  of  the  beam,  we  get  a  set  of  nonlinear  equations  in  which  the  order  is 
twice  the  number  of  elements  in  the  finite  approximation.  Taking  the  approach 
of  Brockett  and  Isidori,  we  could  base  our  control  system  design  on  this  nonlinear 
model,  by  first  finding  the  feedback  which  linearizes  this  system  using  differential 
geometric  methods  and  then  using  a  linear  feedback  to  control  the  system. 

The  Galerkin  model  with  linearizing  feedback  requires  a  full  state  vector  at 
all  times  and  would  require  a  very  complex  nonlinear  state  observer.  It  was  felt 
that  the  equations  which  linearize  the  system  would  not  be  implementable  on 
the  digital  signal  processor  which  has  somewhat  limited  memory,  therefore,  we 
chose  to  implement  a  linear  feedback  system  based  on  the  linear  system  open 
loop  transfer  function  if  we  could  verify  that  this  was  an  acceptable  model. 

We  intend  to  use  the  Galerkin  model  in  simulation  to  show  that  the  linear 
systems,  both  experimental  and  simulated,  give  very  similar  results  which  will 
justify  our  use  of  a  linear  system  in  the  actual  implementation.  To  verify  this 
approach,  we  compared  the  openloop  system  responses  to  a  pulse  input  for  the 
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following  three  systems. 


1.  the  Nonlinear  Galerkin  Model  simulation, 

2.  the  Empirical  Transfer  Function  Estimate  (ETFE)  simulation, 

3.  and  the  Experimental  DATA. 

Before  we  show  the  results,  let  us  develop  the  equations  which  describe  the 
nonlinear  Galerkin  model. 


3.4.1  Galerkin  Model  with  3  Rigid  Bodies 


We  consider  a  flexible  arm  with  some  unknown  parameters  such  as  mass  distri¬ 
bution  along  the  rod,  hub  inertia,  joint  friction,  rod  stiffness.  The  flexible  arm 
is  governed  by  the  Euler  Bernoulli  beam  equation  which  is  derived  in  [5] 


d2 

dx2 


d2y(x , t) 
dx 2 


— m(: r) 


d2y(x,  t) 
dt 2  ’ 


(3.4.26) 


This  equation  is  easily  solved  for  the  case  of  uniform  mass  function  m(x),  but  in 
the  general  case  it  is  not  easily  solved.  Part  of  the  difficulty  lies  in  the  problem 
of  finding  the  mode  shapes  when  the  mass  is  nonuniform. 

We  can  make  an  finite  approximation  to  the  Euler  Bernoulli  beam  equation; 
the  beam  is  broken  down  to  a  series  of  links  which  consists  of  N  rigid  bodies 
coupled  together  with  elastic  springs  at  revolute  joints. 

The  model  used  to  approximate  the  continuum  model  will  be  a  3  body  ap¬ 
proximation  which  consists  of  one  body  for  the  hub  and  two  rigid  bodies  forming 
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the  flexible  beam.  The  rigid  bodies  are  connected  by  a  rotary  joint  and  coupled 
only  with  a  torsional  spring  which  depends  linearly,  with  spring  constant  A:,-,  on 
the  angle  between  the  two  rigid  bodies.  Each  link  has  a  center  of  mass  located 
at  some  point  along  the  ith  rigid  body  which  will  be  denoted  by  e,  where  e,-  rep¬ 
resents  where  the  mass  is  placed.  If  the  mass  is  placed  at  the  joint  nearest  the 
base,  then  et-  =  0;  if  the  mass  is  placed  at  the  distal  end  of  a  link,  then  et-  =  1, 
thus  et-  €  [0, 1].  Also  each  link  has  a  moment  of  inertia  which  is  specified  by  /,■ 
about  the  center  of  mass  of  the  ith  link. 


fi{9i,  =  J2  rj€j  cos(0;)  +  rj~  i(l  -  <b-i)  cos(6'i_1) 

i= i 

i 

9i(0i,...,0i)  =  53rieisin(0j)  +  ri-i(!  ~  sin(6ly_1),  (3.4.28) 

Here  is  found  by  noting  that  in  the  limit  as  N  oc  we  have  that, 
kii'i  — »  El,  niijri  — >  p  while  Yli  ri  =  L  and  YLimi  —  Let  us  also  define  the 
notation,  =  /  =  L/2  for  our  3  body  approximation  and  this  gives  us  a  relation 
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Using  the  symbolic  mathematics  tool  Macsyma  [8],  these  equations  reduce 
for  the  case  when  N  =  3  to 

Tmotor  —  I\h  ~  &i($2  ~  $1) 

0  =  (I2  + 1 elm-2  + 11713)62  +  le3m3  cos(03  -  9 2)6:1 

— /e3m3sin(03  —  92)dl  —  k2{93  —  $2)  +  k\{92  —  $i) 

0  =  lezm3  cos (03  -  92)92  +  /(e3m3  +  /3)03 
+le3m3  sin(03  —  92)(02)2  +  ^'2(^3  —  #2) 

The  equations  of  motion  for  the  case  when  N  =  3  of  the  rigid  body  approx¬ 
imation  of  the  beam  and  hub  system  can  be  written  in  a  vector  notation  as  in 
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[9]  where  Craig  applies  this  equation  to  a  robotic  manipulator. 


h 

Ox 

t  =  M(due2,93) 

h 

+  V(Qi,92,93,9i,92,93)  +  K 

9-2 

h 

Oz 

where  we  have  the  so  called  mass  matrix, 


M(0)  = 


the  stiffness  matrix, 


h  0  0 

0  I2  +  m3!2  +  l2e3m3  cos(93  —  92 

0  l2e3m3  cos(03  -  9o)  I3  +  e\m3l2 


I<  = 


k\  — k\  0 
— k\  k\  -f-  ko  — ko 
0  — k2  k2 


and  the  torque  vector, 


T  = 


Tr 


motor 


If  we  define  the  orientation  vector  0  as 


(3.4.29) 


©  = 


e3 


then  the  second  term  of  the  above  equation  can  be  written  as  a  matrix  multiplied 


by  a  vector,  as  shown  in  [9].  This  allows  us  to  represent  the  second  term  of  the 
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right  hand  side  of  3.4.29  as  the  product  of  a  matrix  and  a  joint  angular  velocity 
vector, 

V(0,0)  =  Vm(0,©)©, 


where  the  subscript  m  stands  for  matrix.  This  term  Vm(0,0)  represents  the 
centrifugal  and  coriolis  force  terms  which  are  fully  represented  in  this  model. 
This  matrix  then  takes  the  following  form: 


Vm(Q,  0)  = 


0 


0 


0 


0  0  —  sin($3  —  $9)^3 

0  /2e3rn3  sin(#3  -  62)62  0 


(3,4.30) 


Thus  we  have 


61 

6l 

61 

T  =  M(0) 

62 

+  Kn(©,©) 

62 

+  I< 

62 

h 

63 

63 

describing  the  Galerkin  model  dynamics. 


(3.4.31) 


3.4.2  State  Space  Equations 

It  is  desired  to  reformulate  the  equations  of  motion  so  they  are  in  the  form: 

x  =  f(x)  +  g(x,  u ).  (3.4.32) 

If  we  rewrite  3.4.29  as: 

M(0)0  =  -K(0,0)-/\0tT  (3.4.33) 
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and  then  since  the  mass  matrix  is  positive  definite, 

0  =  M(0)-1  (-V(0,  0)  -  KQ  +  T)  .  (3.4.34) 

Then  if  we  define  the  state  vector  as: 


The  nonlinear  state  equations  become 

Oi  =  wi 

02  =  U)  2 

$3  =  U>3 

=  y-(#2  -  &i)  +  ~r 

h  h 

(1)2  =  |e3^2m3  cos ($3  —  92)(e3l2m^u)2  sin(03  -  do)  +  ko{6 3  —  #2)) 

+(€3/2m3  +  /3)(€3/2m3cj3  sin(03  —  %)  +  ^2(^3  -  #2)  —  ^l(02  -  $i)}  /A(x) 
W3  =  €3l2m3  cos(03  —  02)(^2m3^3  sin(03  -  do)  +  ko(&3  —  Go)  -  ki(0o  —  0i)) 

~(l2m3  +  €9 /2m2  +  /2)(e3^2m3w2  sin(03  -  02)  +  ^(#3  ~  #2))}  /A(x) 

where 

A(x)  =  (l277r3  +  elfimo  +  I2)(e3J2m3  +  h)  ~  ^A'ml  cos2(03  -  do). 
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Let  us  define  the  following  notation  for  f(x)  and  g  ; 

Q\  u)\  0 

$2  U>2  0 

#3  <^3  0 

=  + 

*1  Wuh)  /f1 

^2  /2(^1,  ^2)  ^31^2,  ^3)  0 

^3  /3(^1,  #2>  #3>  ^2,^3)  0 

3.4.3  Output  Equations 

The  three  outputs  connected  to  the  system  are  the  optical  shaft  encoder,  shaft 
tachometer,  and  tip  accelerometer.  The  equations  for  the  encoder  and  the 
tachometer  outputs  are  just  the  states  9\  and  9\  respectively. 

Tip  Accelerometer 

To  find  the  tip  accelerometer  output  equation,  we  first  write  the  equation  for 
the  forward  kinematics  of  the  3  links.  The  tip  position  is  related  to  the  base 
coordinates  by  the  following  expressions  for  the  x  coordinate  and  y  coordinate 
respectively. 

3 

Y.  Tj€j  cos  6j  +  r,_i(l  —  ej_i)cos0j_i  (3.4.37) 
j= 1 
3 

Y.  rjej  sin 9j  +  rj- i(l  —  €j- 1)  sin  Oj_i  (3.4.38) 

j=i 


f 3(63,02,61)  = 
93(63,  62,61)  = 


(3.4.36) 
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Thus  for  our  case,  we  have  ry  =  0,r2  =  =  l  and  we  can  assume  for  now 

that  €j  =  1.  The  endpoint  position  vector  X  6  R3  of  the  tip  with  respect  to  the 
base  is, 


X(03,02)  = 


The  first  derivative  is, 


X(91,92,9U92)  = 


l  COS  6-2  +  l  COS  #3 

l  sin  d2  +  l  sin  d3 

0 


—19 2  sin  9 2  —  19 z  sin  ^3 
W2  cos  9-2  +  W3  cos  93 
0 


and  the  second  derivative  is, 


X{9u92AAAA)  = 


—19%  cos  92  —  19 2  sin  92  —  19*  cos  93  —  W3  sin  93 
— 19 \  sin  92  +  W2  cos  02  —  19*  sin  93  +  W3  cos  93 

0 


Now  since  the  tip  accelerometer  only  measures  acceleration  normal  to  the  third 
link,  we  must  form  the  dot  product  of  the  acceleration  vector  with  the  vector 


normal  to  the  third  rigid  body, 


N(93)  = 


—  sin  (?3 

COS  6*3 
0 


i.e. 


atip  =  X(9i,92,9i,  92 ,  9\ ,  9 2 )  ■  N(93 ) . 
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After  some  cancellation  and  use  of  trigonometric  identities,  the  acceleration 
reduces  to 

a  up  =  19  2  sin(#3  -  d2)  +  W2  cos  (03  -  d2)  +  W3.  (3.4.39) 

In  the  equation  3.4.39,  the  accelerometer  output  is  not  a  function  of  the 
states  exclusively,  since  there  are  the  terms  #2and  #3-  First  we  can  write  the  tip 
accelerometer  using  some  of  the  other  state  variables, 

atip  =  lu>l  sin(#3  -  02)  +  lto2  cos (03  -  02 )  +  /cb3. 

and  then  we  notice  in  the  state  equations  3.4.36  that  since  the  input  u  is  only 
in  the  fourth  state  equation.  We  can  substitute  in  /2()  and  /3()  for  tu2  and  <h3, 
respectively  yielding  finally 

auP  =  lulsin(93  -  d2)  +  6*2, ^3,^2, w3) cos(6>3  -  92)  +  lf3{9u92,93,u2,u3). 

Here  we  have  the  output  as  a  function  of  states  only  and  not  any  state 
derivatives  and  now  the  equations  of  motion  conform  to  the  the  general  nonlinear 
system  3.4.32. 

3.4.4  Simulation  of  the  Nonlinear  Galerkin  Equations 

The  pulse  response  of  the  3-body  equations  3.4.36  was  simulated  using  Simnon 
[10]  and  the  output  of  the  hub  position  (flj),  rate  (cjx)  and  tip  accelerometer  is 
shown  in  Figures  3.2  through  3.4.  These  can  be  compared  to  both  the  pulse 
response  of  the  linearized  model  simulated  in  the  next  section  and  the  empirical 
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12  . 


pulse  response.  In  addition,  the  pulse  response  of  the  angles  of  the  first  and 
second  links,  02  and  d2  respectively,  are  simulated  in  Figure  3.5.  In  this  plot, 
the  ”1”  corresponds  to  92  and  the  ”2”  corresponds  to  #3. 

3.4.5  Linearization  of  the  Galerkin  Model 

A  model  such  as  the  nonlinear  Galerkin  model  will  includes  the  coriolis  and 
centrifugal  terms  and  is  thus  better  suited  for  modeling  flexible  beams  under 
large  deformations.  Our  system,  on  the  other  hand  has  only  small  deformations 
caused  by  the  high  stiffness  of  the  beam  so  we  can  safely  neglect  these  terms 
and  consider  a  linearized  model. 

One  approach  would  be  to  linearize  the  nonlinear  Finite  Element  model 
around  a  stable  equilibrium  point.  A  stable  equilibrium  point,  is  simply  the 
beam  in  a  relaxed  state  which  is  given  by  0\  =  (j2  —  <111(1  0\  —  02  —  0\\  —  0. 
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Figure  3.3:  3  Body  Pulse  Response  of  Hub  Rate  oji 


I 

I 


Figure  3.4:  3  Body  Pulse  Response  of  Tip  Acceleration  alip 
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Figure  3.5:  3  Body  Pulse  Response  of  angles  of  link  1  and  link  2,i.e.  62  and  9 3 

Since  we  have  a  finite  dimensional  system,  we  can  use  the  truncated  Taylor 
expansion  of  the  nonlinear  system  to  determine  the  linearized  system 

x  +  b  u.  (3.4.40) 

X  =  XQ 

This  linearization  results  in  a  linear  system  of  the  form 


x  =  Ax  4-  b  u 

around  the  chosen  equilibrium  point 


Xo 


T 


0  0  0  0  0  0 


(3.4.41) 


(3.4.42) 
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where  A  is 


0  0  0  1  0  0 
0  0  0  0  1  0 
0  0  0  0  0  1 


0  0  0 


fci(fjjf3m3+/3)  (  —  ki  —  ki)(e^l3ms+l3)—  e^k^m^  000 

A  A  A  U  U  U 

—  €afci  f3ma  fcaQ3m3-ff3t3m3 +/;])  —  e(  —  ki  —  ki)Pm$  fcaCt3  Mi+(%m^+In}  —  k3kA^  m3  0  0  0 

A  A  A  U  U  U 


where 


A  =  (/2m3  +  e2  /2m2  +  *2)(4^m3  +  *3)  —  e3/4m2 

The  hub  mass  and  inertia  are  easily  computed  from  the  hubs  physical  dimen¬ 
sions.  The  hub  has  a  radius  r  =  11.4cm,  a  height  of  h  =  0.87 cm  and  is  made 
of  aluminum  which  weighs  u;  =  26.6 kN/m3.  The  mass  density  of  the  aluminum, 
hub  is  thus  pai  =  26.6  x  103/9.806  =  2.71262  x  10 3kg/m3.  Since  the  hub  is  very 
nearly  a  flat  cylinder,  the  total  mass  is  computed  mx  =  pAi^p2h  =  0.8971  and 
the  inertia  is  computed  as 


Ii  =  mi  —  —  5.427  x  10  3kg/m3. 

Z 


The  mass  of  both  the  second  and  third  link  is  computed  from  the  mass  density 
and  the  dimensions  of  the  beam  elements 


m2  =  m3  =  (2712.62)(0.0030625)(0.04828)(0.5)  =  0.20054. 


The  stiffness  of  the  aluminum  beam  is  computed  by  noting  that  the  modulus 
of  elasticity  for  aluminum  is  E  =  71.0 GPa  and  the  dimensions  of  the  beam  are 


width  w  =  0.30625cm,  height  h  =  4.826cm,  and  length  L  =  100.0cm. 
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The  inertia  about  the  first  y-axis  joint  is 


and  is  thus 


PAihw 

24 


h  =  h  = 


(0.0030625)(0.04826)(2.7162) 

24 


=  1.6705  x  10~5 


and  we  have  =  e2  =  §• 

With  these  particular  parameters  for  the  flexible  beam  experiment,  the  lin¬ 
earized  Galerkin  model  has  eigenvalues  which  correspond  to  the  vibratory  modes 
of  the  system: 


Ao 

Ai 

A2 


=  0  (multiplicity  2) 

j75.20 

~  y/2 

j  170.17 

V2 


from  which  we  can  compute  the  modal  frequencies 


fo  =  0 
fi  =  8.46 
h  =  19.15 

using  the  relation  A  =  2nf. 
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3.5  Empirical  System  Identification 


3.5.1  Empirical  Transfer  Function  Estimate 

To  determine  the  mathematical  model  of  the  open  loop  system  and  verify  some 
of  the  results  of  Frank  [1],  some  linear  system  identification  procedures  were 
done  on  the  experimental  hardware  to  find  the  frequency  response  of  the  system 
excited  by  a  impulse.  This  methodology  assumes  that  the  system  is  linear. 

To  obtain  the  Bode  plots  of  the  system,  the  following  methodology  is  per¬ 
formed  on  each  sensor  output.  An  impulse  was  approximated  by  applying  the 

maximum  motor  torque  for  one  sampling  period  (in  this  case,  Ts  =  0.01)  to  the 

*  » 

motor  which  excites  the  system.  The  outputs  of  the  tachometer  and  accelerom¬ 
eter  were  sampled  via  the  analog  to  digital  converter  (ADC)  board  on  the  IBM 
PC.  The  1024  point  Discrete  Fourier  Transforms  (DFT)  of  these  sampled  out¬ 
puts  were  then  determined  using  a  subroutine  from  the  DSP32  Software  Library 
[11]  on  the  DSP32  chip.  The  magnitude  (converted  to  dB)  and  phase  of  the 
DFT  approximate  the  Bode  plot  of  the  motor  input  to  the  corresponding  sensor 
output.  This  approximates  the  transfer  function  of  the  openloop  system  from 
which  we  will  base  the  control  system  design. 

One  problem  is  that  we  can  not  take  the  DFT  of  the  encoder  to  obtain  the 
Bode  plot  for  the  hub  position  because  the  DFT  requires  that  you  have  a  time 
sequence  which  is  of  finite  length  i.e.,  there  exists  some  N  such  that  f(k)  =  0 
for  k  >  N.  Both  the  tachometer  and  accelerometer  time  sequences  satisfy  this 
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condition.  To  obtain  the  transfer  function  for  the  hub  position  we  must  divide 
the  transfer  function  obtained  from  the  tachometer  by  the  Laplace  variable  s. 


3.5.2  Obtaining  the  Pole-Zero  Transfer  Function  from  the  Empir¬ 
ical  Data 


Since  we  have  data  for  the  frequency  response,  we  would  like  to  obtain  a  transfer 
function  H(s)  which  approximates  the  empirical  transfer  function  II (,s).  The 
goal  of  this  experiment  is  to  model  the  infinite  dimensional  system  by  a  six 
order  finite  dimensional  system.  In  addition  we  will  need  a  second  order  transfer 
function 


«, /(») 


+  a*  +  i 


(3.5.43) 


to  model  the  analog  prefilter  which  is  placed  after  the  sensors  to  reduce  aliasing. 
The  cascade  of  both  transfer  functions  results  in  an  eighth  order  system.  We  will 
perform  the  curve  fitting  for  both  the  sixth  order  model  without  the  prefilter  in 
cascade  and  the  eight  order  model  which  includes  the  prefilter.  A  sixth  order 
model  would  be  preferable  if  it  a  gives  reasonable  estimate  of  the  DFT. 

One  possible  method  of  determining  the  transfer  function  would  be  to  con¬ 
struct  an  optimization  procedure  in  which  the  mean  square  error  is  minimized. 
The  parameters  which  minimize  the  sum  of  the  mean  square  error  of  the  ETFE 
for  each  sensor  describes  the  poles,  zeros,  and  damping  ratios  of  the  system. 

From  Chapter  3,  we  have  considerable  prior  knowledge  as  to  the  form  or  ex¬ 
pected  form  of  the  open  loop  transfer  functions.  Also  the  alternation  property 
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Figure  3.6:  Impulse  Response  of  Tip  Accelerometer 


of  the  poles  and  zeroes  will  be  helpful  in  fitting  the  curves.  Since  we  are  con¬ 
structing  a  low  order  controller,  and  with  this  knowledge,  it  is  quite  reasonable 
to  simply  construct  the  ETFE  by  hand.  From  the  accelerometer  data,  we  can 
determine  the  poles  of  the  system  and  adjust  the  zeros  to  best  match  the  mag¬ 
nitude  and  phase  of  the  system.  Care  was  taken  to  match  the  phase  as  much  as 
possible.  The  damping  ratio  of  each  pole  was  adjusted  by  matching  the  slope  of 


the  ETFE  phase  response  with  that  from  the  experimental  phase  response. 

The  estimate  for  the  transfer  function  from  the  motor  input  to  accelerometer 


Hac(s)  = 


- 1]  f(^)!-(£S)-M _ 

Ksfa)  +  4  (<nh)’  +  (Sf)  + 1]  [<cte)’  +  (Sft) 


(3.5.44) 


and  the  comparison  of  this  estimate  to  the  experimental  data  is  given  in  figure 


3.7.  The  tachometer  transfer  function  has  the  same  poles  as  the  accelerometer 
transfer  function  so  we  only  need  to  fit  the  numerator  to  the  experimental  data. 
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Figure  3.7:  DFT  of  Tip  Accelerometer 
This  yields  after  curve  fitting  3.9,  the  transfer  function 


130s 


Hv(s )  = 


(sh)! + (?#« + 


+  1 


Usfe) + 1]  + <ta> + ']  [(^ + <sfe> + 


(3.5.45) 


Since  the  optical  encoder  is  collocated  to  the  same  shaft  as  the  tachometei, 
the  transfer  function  for  the  encoder  is 


Hp(s)  =  k-H„(s), 


(3.5.46) 
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Figure  3.8:  Impulse  Response  of  Hub  Angular  Velocity 

where  k  is  a  scale  factor  due  to  the  different  gains  of  the  sensor  outputs.  The 
scale  factor  k  was  found  to  be  15.0,  thus  we  have  the  encoder  transfer  function, 


130xl5[(2;24)2  +  (2°^)+l' 

[( 

s  *  /  0.06s  \  j  1 

2*17.2  /  1  12*17.2  7  'r  1 

s  [(2*0.25)  + !]  [(2*8.2)  +  (2*8.2)  + l] 

\t  s  \2  |  C  0.06*  \  .  1 
[12*19.8/  1  1  2*19.8  /  ^ 

The  transfer  function  for  the  analog  prefilter  is 


ffp/M 


(3.5.48) 


although  this  was  not  included  in  the  final  model  since  we  want  to  limit  the 
order  of  the  controller.  Satisfactory  results  are  given  by  the  sixth  order  model 
and  the  curve  fit  does  not  warrant  an  increased  order.  In  fact,  if  an  eighth  order 
model  was  to  be  approximated  with  the  best  curve  fit,  a  superior  approximation 
could  be  obtained  by  cascading  in  series  the  second  order  term  for  the  next  mode 
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Figure  3.9:  DFT  of  Hub  Tachometer 

instead  of  including  the  prefilter  transfer  function  to  our  sixth  order  model. 

3.5.3  Simulation  of  ETFE 

The  impulse  response  of  the  ETFE  continuous  time  model  is  verified  by  simu¬ 
lating  the  pulse  response  of  the  open  loop  model  for  each  of  the  three  outputs 
as  well  as  the  tip  position,  figures  3.11  through  3.14,  and  and  comparing  the 
results  to  the  empirical  pulse  responses. 
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Figure  3.10:  Impulse  Response  of  Hub  Position 
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Figure  3.11:  Pulse  Response  of  Open  Loop  Model-  Accelerometer 


Figure  3.12:  Pulse  Response  of  Open  Loop  Model-  Tachometer 

3.6  Disturbances 

3.6.1  Friction  Compensation 

The  motor  and  hub  assembly  has  a  considerable  amount  of  friction  which  must 
be  modeled  and  compensated  for.  The  system  has  Coulomb,  static  friction, 
viscous  friction  as  well  as  air  drag  on  the  flat  beam.  Coulomb  and  static  friction 
can  be  lumped  as  a  single  bearing  friction  while  viscous  friction  is  a  linear 
function  of  hub  rate. 

Tuf  =.kvf0  (3.6.49) 

Caution  must  be  used  when  implementing  the  viscous  friction  compensator 
because  the  system  could  go  unstable  if  we  overcompensate  with  a  higher  value  of 
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Figure  3.13:  Pulse  Response  of  Open  Loop  Model-  Encoder 

kvf.  Ideally,  we  would  like  to  compensate  the  viscous  friction  so  as  to  eliminate 
it  entirely,  but  if  the  value  for  the  coefficient  is  too  large,  the  system  will  be 
unstable,  so  we  must  tolerate  a  small  amount  of  viscous  friction  to  give  us  a 
safe  stability  margin  by  slightly  undercompensating.  This  instability  can  be 
explained  because  if  we  were  to  implement  a  high  gain  friction  compensator, 
we  would  be  supplying  more  energy  to  the  system  than  the  friction  would  be 
dissipating. 

A  simple  method  to  determine  the  value  of  the  coefficient  is  to  write  a  pro¬ 
gram  that  compensates  for  the  friction  while  we  adjust  the  coefficient  to  be  a 
large  as  possible  but  also  requiring  the  motor  to  have  a  small  stability  margin. 
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cubed  term  for  the  air  drag  compensator.  With  Kvj  =  —1.05  ,  the  hub  and 
beam  system  is  stable  yet  it  behaves  as  very  nearly  a  frictionless  bearing  at  all 
allowable  hub  rates. 

3.6.2  Ripple  Torque  Compensation 

The  motor  has  some  undesirable  ripple  torque  which  we  would  like  to  eliminate 
so  that  it  doesn’t  excite  any  vibratory  modes  of  the  flexible  beam  as  we  slew  the 
robot  arm.  The  ripple  torque  is  seen  by  applying  a  constant  small  input  signal 
to  the  motor  (  u  =  40out  of  a  maximum  of  2000),  (which  is  enough  to  cause  the 
hub  to  rotate  at  a  moderately  slow  rate),  then  the  velocity  is  measured  as  the 
ripple  torque  acts  on  the  system.  For  observing  the  ripple  torque,  we  do  not 
want  to  compensate  for  viscous  friction.  With  a  constant  input  and  the  friction 
acting  on  the  system  the  beam  will  reach  a  constant  steady  state  velocity.  Any 
detectable  variation  in  speed  can  then  be  attributed  to  ripple  torque  acting  on 
the  motor  shaft. 

Figure  3.15  shows  that  the  hub  rate  is  not  constant  as  the  motor  rotates  from 
—tv  to  7 r  while  figure  3.16  shows  more  detail.  The  variation  in  hub  rate  is  really 
only  detectable  at  very  slow  speeds  while  at  high  speeds  there  is  almost  basically 
no  ripple  torque  visible.  Also  seen  in  these  graphs  is  that  the  magnitude  of  the 
velocity  vaxiation  varies  as  a  function  of  hub  position. 

Due  to  the  sinusoidal  envelope,  the  ripple  torque  must  be  compensated  with 
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Figure  3.15:  Variation  of  Velocity  with  respect  to  Position, 
the  compensating  function 

u  =  a(l  —  p  cos  (0  —  <p))  sin(ce#  —  ip)  (3.6.50) 

A  compensator  program  was  implemented  which  applied  the  feedforward  law 
with  the  variables  listed  in  Table  3.1,  and  the  compensator  showed  only  a  neg¬ 
ligible  improvement.  Other  parameters  were  tried  as  well  with  most  giving  the 
same  if  not  worse  performance. 

The  ripple  torque,  being  a  sinusoidal  function,  has  a  spatial  frequency  of  41 
cycles  in  one  revolution  of  the  hub  which  is  caused  by  the  poles  of  the  motor 
magnets.  In  each  cycle  we  have  a  both  a  stable  and  an  unstable  position  with 
zero  ripple  torque  as  well  as  the  peak  values  of  ripple  torque  between  these 
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Figure  3.16:  Velocity  Change  in  One  Revolution 


Variable 

Description 

Value 

a 

gain 

20.0 

P 

Variation 

0.16 

9 

motor  position 

0 

envelope  offset 

1.0 

(jJ 

ripples  per  revolution 

41 

ripple  offset 

2.1 

Table  3.1:  Ripple  Torque  Variables 
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points.  To  measure  the  maximum  ripple  torque  we  find  the  maximum  applied 
motor  torque  such  that  the  hub  remains  static  in  the  stable  portion  of  one  ripple 
cycle.  Experimentally,  we  slowly  increase  the  amount  of  applied  motor  torque 
in  small  increments  until  the  shaft  goes  beyond  a  stable  point  and  jumps  into 
the  next  cycle. 

The  conclusion  of  this  experiment  was  that  a  motor  signal  of  5  caused  the 
hub  to  remain  in  equilibrium  with  the  ripple  torque  and  any  signal  higher  caused 
the  hub  to  start  rotating.  Thus  relative  to  the  maximum  motor  output  of  ±2048, 
the  ripple  torque  is  almost  negligible.  Perhaps,  this  is  why  the  compensation 
showed  little  improvement  in  measured  velocity  variation.  In  effect,  the  feed 
forward  term  of  the  ripple  torque  compensator  takes  values  between  ±5  and 
since  we  are  working  with  integers,  we  have  a  rather  coarse  quantization  effect. 

Also,  the  output  of  the  encoder  sensor,  figure  3.17,  suggests  that  the  hub 
rate  is  considerably  smoother  than  3.16  suggests.  Although  some  actual  ripple 
torque  has  been  measured,  some  of  the  variation  in  hub  rate  could  be  due  to  the 
tachometer  instead  of  the  actual  hub  rate  since  it  operates  via  magnetic  poles 
just  as  the  motor  does. 

With  the  negligible  improvement  of  the  ripple  torque  compensator,  we  chose 
not  to  implement  it  in  the  final  closed  loop  system  and,  in  fact,  no  induced 
vibrations  were  noticed  in  the  final  step  response  of  the  closed  loop  system. 
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Control 


4.1  Nonlinear  Control 

4.1.1  Input  Output  Linearization 

We  consider  the  nonlinear  system  with  one  input  described  by 


x  =  f(x)+g(x)u  (4.1.1) 

V  =  h{x)  (4.1.2) 

and  we  would  like  to  linearize  this  system  with  respect  to  the  input  output 
response.  A  system  of  N  connected  rigid  bodies  was  treated  by  Sreenath  [12] 
for  the  case  of  no  external  torque  and  Posbergh  [6]  gives  examples  of  the  Input 
Output  Linearization  methodology  for  the  case  of  a  2-body  and  a  3-body  chain 
of  rigid  bodies  (see  pages  131-142).  These  examples  linearize  the  equations  with 
respect  to  one  of  the  outputs,  namely,  the  hub  rate.  A  hub  position  sensor 
output  would  also  be  linear  as  well  since  it  is  collocated  to  the  hub  rate  sensor, 
and  therefore,  the  I/O  linearized  arm  could  be  controlled  using  linear  feedback. 
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The  I/O  Linearization  method  is  based  on  chapter  5  of  Isidori  [13]  and  lin¬ 
earizes  the  system  by  way  of  the  feedback 

u(t)  =  a(x)  +  /3(x)v(t).  (4.1.3) 

This  feedback  requires  that  the  complete  state  x  vector  be  known  in  order 
to  carry  out  the  linearization  and  this  emphasizes  the  need  for  a  good  observer 
to  the  nonlinear  system. 

A  system  is  said  to  be  linear  with  respect  to  the  input  and  output  if  the 
relationship  between  y  and  u  can  be  put  in  the  form 

y(t)  =  y(t,x0)+  f  k(t  —  t)u(t)<1t ,  (4.1.4) 

Jo 

where  k(t  —  r)  is  the  first  order  kernel  of  the  Volterra  Series  of  4.1.2. 

Theorem  4.1  (Isidori)  A  necessary  and  sufficient  condition  for  the  system 
4-1.2  to  be  I/O  linearizable  is  that  the  first  order  kernel  of  the  Volterra  Series 

w(t ,  r,  x ) 

depend  on  the  difference  ( t  —  r)  and  does  not  depend  on  x,  in  a  neighborhood  U 
of  the  initial  point  x0. 

Alternatively,  a  necessary  and  sufficient  condition  for  this  kernel  to  be  inde¬ 
pendent  of  x  and  depend  only  on  t  —  t  is  that  the  Lie  derivative 

LgLkhffx)  is  independent  of  x  Vk  >  0, 1  <  j  <  /. 
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In  general,  for  a  specific  nonlinear  system,  this  is  not  satisfied  and  we  may  wish 
to  find  a  feedback  4.1.3  that  satisfies  this  requirement. 

Thus  the  input  output  linearization  problem  can  be  stated  as  follows.  Given 
(/,  g ,  h)  and  an  initial  state  x0,  find  a  neighborhood  U  of  xQ  and  a  pair  of  feedback 
functions  a  and  /?,  defined  on  U  such  that  for  all  k  >  0  and  all  1  <  j  <  / 

LgpLf+gahj(x)  —  independent  of  x  on  U. 

A  convenient  way  to  solve  this  problem  is  to  arrange  the  functions  LgiLjhi(x ), 
which  characterize  the  Taylor  series  expansions  of  the  kernels  Wj(t,  0,ar)  around 
t  =  0,  in  a  /  x  m  matrix  denoted  by  Tk(x), 


Tk(x)  =  [f,y(x)j  (4.1.5) 

where 

t{j  =  LgiL)hi{x). 

We  will  attempt  to  linearize  the  nonlinear  system  with  respect  to  the  chosen 
state  variables  #i,  9%,  O3, u>i,u>2,  W3,  so  temporarily  define  the  output  functions  as 


hi(x) 

0x 

h2(x) 

02 

h3(x) 

03 

h4(x) 

h5(x) 

UI2 

h6(x) 

W3 

51 


For  our  case,  we  have  only  one  input  (m  =  1),  so  Tk(x )  is  just  a  column 


vector  of  the  form 

r 

LgLkfhi(x) 
LgLkh2(x) 
LgLkh3(x ) 
LgLkh4(x ) 
LgLkh5(x) 
LgLkh6(x ) 

which  is  defined  for  the  formal  power  series 


Tk(x)  = 


oo 

r(v)  =  ET*Wi-‘-. 


k-o 


For  our  system, 


Lghx(x)  = 

1 

0 

0 

0 

0 

0 

$(*)  = 

=  0 

Lgh2(x)  = 

0 

1 

0 

0 

0 

0 

9{x)  = 

=  0 

Lgh3(x )  = 

0 

0 

1 

0 

0 

0 

9{x)  = 

-  0 

Lgh4(x) 

0 

0 

0 

1 

0 

1 

0 

9{x)  = 

=  Jf 

Lgh${x)  = 

0 

0 

0 

0 

1 

0 

9(*)  = 

=  0 

Lgh'Q^x') 

0 

0 

0 

0 

0 

1 

g{x) 

=  0 

(4.1.7) 
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thus 

0 
0 
0 

T0(x)  = 

/r1 

0 

0 

Theorem  4.2  (Isidori)  There  exists  a  solution  at  xQ  to  the  Input-Output  Lin¬ 
earization  Problem  if  and  only  if  there  exists  a  formal  power  series 

OO 

ife=o 

whose  coefficients  are  l  X  m  matrices  of  real  numbers,  and  a  formal  power  series 

OO 

R(s,x)  =  R.1  +  YtRk(x)s-k- 1 

fc= 0 

whose  coefficients  are  m  x  m  matrices  of  smooth  functions  defined  on  a  neigh¬ 
borhood  U  of  x0,  with  invertible  R- 1  ,  which  factorizes  the  formal  power  series 
T(s,x )  as 

T(s,x)  =  K(s)R(s,x). 

4.1.2  The  Structure  Algorithm 

Following  the  steps  outlined  in  Isidori  for  the  structure  algorithm  to  find  a 
linearizing  feedback,  we  proceed  with  step  1. 


53 


Step  i  =  1.  We  need  to  find  the  permutation  matrix  V\  which  which  performs 


We  choose: 


where 


=  rank  S\(x) 
=  1 

7i(x)  =  Pih(x) 

=  h4(x) 

= 

7i(z)  =  K\h{  x) 


(4.1.9) 


(4.1.10) 
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hi(x) 
h2(x) 
=  h3(x) 

h5(x) 
h6(x) 

Ox 

d2 

—  e3 

0J2 

IX)  3 

and  note  that 


=  Sx{x) 

0 


Lg  7i(z) 


0 

0 


0 

0 


Now  step  i  —  2.  Consider  the  matrix; 


Lgl\{x ) 

Si-i(x) 

LgLf% 

LgLfix  _ 
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since 


d'Ti  - 


1  0  0  0  0  0 
0  1  0  0  0  0 
0  0  1  0  0  0 
0  0  0  0  1  0 
0  0  0  0  0  1 


and  we  have  the  Lie  derivative  of  71  with  respect  to  /, 


U>2 


Lf  7i(*)  = 


LO3 


/2(^1,  02,  &3,U2,  ^3) 
/•2(01,  #27  ^37^27^3) 


and  then  since 


dLf-ji  - 


0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

dJx 

dOt 

dh 

dd2 

dJz 

de3 

0 

dh 

dw2 

dh 

dtj3 

d& 

d6x 

dh 

dd2 

dh 

de3 

0 

dh 

dw2 

dh 

du/3 
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we  see  that 


LgLtfi(x)  = 


Now  if  we  choose  V2  as 


we  get 


and 


1  0  0  0  0  0 

-110000 
0  0  1  0  0  0 

0  0  0  1  0  0 

0  0  0  0  1  0 

0  0  0  0  0  1 


^2 


IT1 

0 

0 

0 

0 


I<1  K\ 


Lgll{x) 

&(*) 

_  LgLf 7i(*)  _ 

0 

S2(x)  =  Si(s)  =  II 


-1 


so  we  are  done.  Also 


62  -  n  -  r0 
72  (z)  =  does  not  exist 

72  =  +  KlLni{x) 
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/r1 

Ui 

0 

i02 

0 

+ 

U/3 

0 

/2() 

0 

.  /3()  . 

Next  we  form 


[  72(z)  J 

and  then  a  and  ft  is  found  by  the  relationship 


(i»r(x)lor(i)  =  -L,V(x) 

[i,r(x)l  0(x)  =  [11 


so  we  have 

M a!x)  =  -/it1) 
[ir']m  =  i 

and  solving  for  a  and  /3 

a(x)  =  — ii/i(0i,  #2) 

/?(*)  =  h 

results  in  the  linearizing  feedback 

u  —  —  ii/i(#i,  #2)  +  hv- 


(4.1.H) 
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We  have  linearized  the  Galerkin  model  with  respect  to  the  hub  rate  and  hub 
position  but  the  structure  algorithm  degenerated  before  we  could  linearize  the 
system  with  respect  to  the  other  outputs. 

4.1.3  Implementation 

In  general,  the  method  of  feedback  linearization  requires  a  full  state  vector  of 
the  nonlinear  system  and  this  either  requires  a  nonlinear  observer,  which  must 
be  implementable  on  a  processor  of  reasonable  cost  and  having  sufficiently  high 
sampling  rate,  or  some  distributed  array  of  sensors  to  put  on  line  the  measured 
estimates  of  the  states.  In  our  particular  system,  the  linearizing  feedback  re¬ 
quires  only  the  two  states  6 1  and  do  and  is,  therefore;  in  some  sense  simpler. 
Even  a  nonlinear  observer  for  this  case  would  be  difficult  to  implement  on  a 
low  cost  digital  signal  processor  like  the  AT&T  WE  DSP32,  since  it  requires  a 
long  software  subroutine  to  carry  out  most  nonlinear  functions.  On  the  other 
hand,  the  DSP32  is  very  proficient  at  multiplication  succeeded  by  addition  which 
suggests  its  use  for  a  linear  control  system. 

Implementing  the  state  vector  estimator  for  the  nonlinear  system  is  achieved 
in  a  cost  effective  manner  by  use  of  sensors  which  approximate,  in  some  sense, 
the  states  that  are  required  for  feedback.  The  optical  shaft  encoder  measures  ffi; 
but  #2',  the  angle  of  the  first  rigid  element  of  the  Finite  Element  approximation 
to  the  continuous  beam,  is  currently  unavailable.  An  approximation  of  this 
angle  could  be  obtained  through  a  strain  gauge  placed  on  the  beam  near  its 
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connection  to  the  hub.  With  such  a  strain  gauge,  the  required  state  vector  for 
feedback  linearization  would  be  on  line  with  little  delay  due  to  preprocessing 
and  at  a  relatively  low  cost. 

Another  drawback  of  the  nonlinear  control  presented  here  is  that  it  doesn’t 
dampen  the  flexible  modes  of  the  beam;  it  merely  servos  the  hub  to  the  desired 
location.  Since  the  linearized  system  is 

h  =u, 

the  hub  position  could  be  servoed  with  high  speed  using  a  bang-bang  controller, 
but  this  method  makes  no  attempt  to  control  the  end  effector  other  than  through 
the  natural  damping  of  the  beam.  This  could  take  some  time  for  the  tip  position 
to  settle  to  the  actual  angle  indicated  by  the  hub  for  systems  with  little  or  no 
damping.  On  the  other  hand,  such  a  linearized  system  which  servos  only  the 
hub  to  the  reference  position  might  be  very  useful  in  a  satellite  application  which 
has  some  flexible  structures  attached  that  make  control  difficult. 

For  applications  related  to  robotics,  we  are  more  concerned  with  end  effector 
control  than  hub  position  control  and  a  linear  system  of  finite  dimension  could 
model  the  lower  dominant  modes  of  the  system  while  truncating  the  remaining 
modes  which  have  negligible  amplitude.  A  model  of  this  type,  while  only  valid 
for  modest  slew  rates,  will  approximate  the  response  of  a  the  nonlinear  Galerkin 
model. 

A  possible  strategy  for  control  system  design  could  to  use  linear  state  fced- 
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back  which  places  the  closed  loop  poles  to  achieve  sufficiently  fast  response  of 
the  lowest  rigid  body  mode  of  the  hub  while  only  the  damping  of  the  higher 
flexible  modes  is  increased  while  the  frequency  of  these  higher  modes  remains 
unchanged.  Such  a  system  is  implementable  on  a  low  cost  DSP  chip  with  a  high 
sampling  rate  as  this  thesis  demonstrates. 

4.2  Discretization  of  the  Linear  Continuous  Time  System 

The  system  identification  procedures  lead  to  an  open  loop  transfer  function  for 
the  flexible  beam  manipulator  which  describes  the  dynamics  of  the  beam. 

Hac(s) 

Htach  (  &  ) 

Hhub-pos(s) 

H tip— posts') 

To  discretize  the  system  we  use  the  operation 

G(z)  =  (1  -  z~l)Z  C~l  (4.2.13) 

where  Z  signifies  the  Z  transform  operator  and  C~l  represents  the  inverse 
Laplace  operator. 

Since  the  discretization  depends  on  the  sampling  rate,  we  will  need  two 
separate  discretizations, i.e.  one  for  200  Hz  and  one  for  400  Hz.  A  Macsvma 
package  was  written  which,  for  a  given  sampling  rate,  discretizes  the  equations 
and  converts  the  proper  transfer  function  to  a  strictly  proper  one.  Thus  the 
discretization  of  the  openloop  system  takes  the  form: 


(4.2.12) 
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H(z) 


bxz 5  +  b2z 4  +  b3z3  +  b4z2  +  b5z  4-  bG 
z6  +  aiz5  +  a2z4  4-  a3z3  4-  a4z2  +  a5z  +  a6 

and  the  control  canonical  realization 


+  d 


x(k  +  1)  = 


y(k) 


b\  b2  b3  b4  65  b( 5 


4-  du(k) 


is  obtained  and  let  us  define  $,  I\  and  C  as 


(4.2.14) 


" 

— fll 

—a2 

-a3 

— a4 

—as 

-a6 

1 

1 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

x(k)  + 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

u(k) 


x{k  + 1)  =  $x(k)+Tu{k)  (4-2.15) 

y(k)  =  Cx(k).  (4.2.16) 

The  discretized  transfer  functions  for  a  sampling  rate  of  200  Hz  are: 


Hac(z ) 

Htach  (.z) 

Hfiub—pos{z) 
Htip—pos{z ) 


6.57z5  -  25.97 z4  4-  40.16z3  -  29.47z2  -  9.46z  -  0.763  _  3(J  ^ 

z 6  -  5.49z5  4-  12.92z4  -  16.74z3  4-  12.58z2  -  5.2 Iz  +  0.925 
15. 2z5  -  7.10z4  +  136.44z3  -  135.11z2  4-  69.04z  -  14.5 
z6  -  5.49z5  4-  12.92z4  -  16.74z3  4-  12.58z'2  -  5.21z  4-  0.925 
Q,58z5  -  1.57z4  4-  1.02z3  4-  0.876z2  -  1.44z  +  0.53 
z®  -  5.49z5  4-  12.92z4  -  16.74z3  4-  12.58z2  -  5.21z  4-  0.925 
— 0.44z5  4-  1.23z4  -  0.85z3  -  0.66z2  4-  1.2z  -  4.3 


z6  -  5.49z5  4-  12.92z4  -  16.74z3  4-  12.58z2  -  5.21z  4-  0.925 
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Figure  4.1:  Pulse  Response  of  Discretized  Open  Loop  Model-  Accelerometer 

4.2.1  Simulation  of  Discretized  Model 

The  discretized  model  was  simulated  so  the  system  response  could  be  compared 
to  the  response  of  the  continuous  time  model  to  ensure  that  the  discretization 
steps  were  error  free.  The  results  are  in  the  following  figures. 

4.3  State  Feedback  Control  System  Design 

Linear  state  feedback  is  employed  to  relocate  the  poles  of  the  system.  The 
feedback  control  signal  sent  to  the  motor  which  is  computed  at  every  sample  is 
given  by 

u(k)  —  vrrj  —  K:r(k)  (4.3.17) 


G3 
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Figure  4.2:  Pulse  Response  of  Discretized  Open  Loop  Model-  Tachometer 

where  K  is  a  6  x  1  matrix  of  gains.  The  gains  are  chosen  to  maximize  the 
performance  and  the  optimization  of  the  gains  is  computed  .using  Console  [14] . 
The  reference  voltage  vTef  is  the  new  input  to  the  system  and  it  corresponds 
to  the  desired  location  of  the  robot  arm.  For  this  experiment  vref  is  a  unit 
step  input.  The  feedback  control  control  computation  above  is  dependent  on 
the  knowledge  of  the  six  states  in  x  and  to  obtain  these  we  construct  a  state 
estimator  or  observer. 
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Figure  4.3:  Pulse  Response  of  Discretized  Open  Loop  Model-  Encoder 

4.4  State  Estimators 

4.4.1  Full  Order  State  Estimator 

The  full  order  state  observer  is  constructed  using  the  equation 

x(k  +  1)  =  $x(k)  +  r u(k)  +  L[y(k)  -  Cx(k)]  (4.4.18) 

which  is  a  new  dynamical  system  based  on  the  dynamics  of  the  open  loop  system. 
The  new  dynamical  system  is  driven  by  the  same  input  as  the  robot  arm  as  well 
as  a  correcting  term  which  is  based  on  the  difference  between  the  measured  and 
the  estimated  outputs.  The  6x3  matrix  L  is  chosen  such  that  the  estimate  of 
the  states  has  adequate  convergence  to  the  actual  states  of  the  system. 
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Figure  4.4:  Pulse  Response  of  Discretized  Open  Loop  Model-  Tip  Position 
The  observer  equation  above  reduces  to 

x{k  +  1)  =  ($  —  LC)x(k)  +  Tu(k)  +  Ly(k) 
and  since  we  have  computed  u  above  we  get 

x(k  +  1)  =  ($  -  LC  -  TK)x(k)  +  Tvref  +  Ly(k)  (4.4.19) 

This  discrete  time  dynamical  system  is  computed  between  every  two  consecu¬ 
tive  samples  to  update  the  states  for  use  in  the  feedback  equation  above.  The 
observer  4.4.1  is  a  sixth  order  linear  filter  with  infinite  impulse  response,  having 
a  polynomial  transfer  function  and  since  DSP  chips  are  excellent  at  filtering, 
they  should  be  also  be  excellent  at  feedback  control. 
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4.4.2  Reduced  Order  State  Estimator 


Since  we  directly  measure  the  outputs  of  three  sensors,  all  of  this  sensor  infor¬ 
mation  can  be  utilized  by  the  observer  to  reduce  the  order  of  the  observer.  We 
can  reduce  the  order  of  the  state  estimator  from  six  for  the  full  order  observer 
to  3  for  the  reduced  order  observer,  This  is  possible  since  the  output  matrix  C 
has  rank  3,  i.e.  all  three  outputs  are  linearly  independent.  A  change  of  basis 
of  the  system,  will  result  in  three  states  being  transformed  to  a  new  system  in 
which  the  sensor  outputs  correspond  to  the  first  three  states  in  the  state  vector 
x,  i.e. 


C  = 


03 


while  the  remaining  3  states  do  not  represent  any  output  but  are  estimated  by 
the  observer. 

There  is  a  trade  off  when  constructing  reduced  order  observers  which  is 
caused  by  sensor  noise.  Reduced  order  observers  have  the  benefit  of  less  com¬ 
putational  steps  but  suffer  if  the  sensor  outputs  are  noisy  since  this  noise  is  fed 
back  into  the  plant.  This  could  be  a  problem  for  noisy  sensors  like  the  tachome¬ 
ter  and  particularly  the  accelerometer,  but  with  adequate  filtering  of  the  signals 
before  sampling,  the  control  signal  u  should  not  be  adversely  corrupted  by  the 
sensor  noise. 

The  reduced  order  observer  is  constructed  by  first  making  a  change  of  basis 
of  the  state  space.  The  transformation  matrix  is  defined  as 
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c 

p  = 

R 

where  R  is  an  3  x  6  arbitrary  submatrix  of  real  constants  such  that  P  is  non¬ 
singular.  The  inverse  of  P 

Q  =  p'l=  Qt  :  Qt 

is  needed  as  well.  Here  Qi  and  Q2  are  both  3x3  matrices.  Next  the  control 
canonical  realization,  obtained  previously,  is  transformed  by  x  —  Px  so 

x(k+  1)  =  P$p-lx(k)  +  PTu(k) 

y(k)  =  CP~lx(k)  =  CQx(k)  =  /3  q3  %(k) 

Now  the  transformed  system  is  partitioned  as 

Xi(k  +  l)  $11  $12  X\(k)  fi 

—  4-  u(k) 

X2{k  +  1)  $21  $22  a’2  (k)  f  2 

y(k)  =  j3  o3  *(k)  =  *i(k) 

Here  aq  is  the  first  3  elements  of  x  and  a2  is  the  latter  3  elements  of  x.  Using 
the  output  equation  y  =  oq  as  well  as  introducing  the  shift  operator  notation  ( 
qf  =  f{k  +  1)  ),  we  can  rewrite  4.4.20  as 

qy  =  $ny  +  $12.^2  +  fYu 

q$2  =  $22^2  +  $liy  +  f2W. 
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and  if  we  define  two  new  variables 


u  =  <&2i2/  +  r2« 

w  =  qy-$uy-riU 

the  system  becomes 


qx2  =  $22x2  +  u 

(4.4.20) 

w  =  <&12x2. 

(4.4.21) 

Here  u  and  w  can  still  be  thought  of  as  input  to  a  linear  system  (even  though 
they  contain  y  and  qy  )  because  they  are  known  signals  driving  the  observer. 


Theorem  4.3  The  pair  {<h,C}  is  observable  if  and  only  if  the  pair  {4>22,  $12} 
is  observable.  (<§22  is  (n  —  q)  x  (n  —  q)  and  $12  is  q  x  (n  —  q)) 


Proof 


The  pair  {<&,C}  is  observable  <£=4- 


rank 


si  -  $ 
C 


=  n  Vs  €  ( C. 


rank 


si  -  <&u  -<b 


12 


-$21  si  -  4> 


22 


=  n  Vs  6  (C 


rankl,  4-  rank 


—  4?  12. 
si  —  $90 


Vs  g  (C 
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=  q  -f  rank 


si  —  $22 
$12 


Vs  e(C 


Thus  the  pair  {$,C}  is  observable 


si  —  $22 
$12 


—  n  —  q  Vs  £  ( D 


{$22,  $21}  is  observable. 

□ 

Since  the  system  4.4.21  is  observable  there  exists  a  third  order  estimator  of 
£2  in  the  form 

qx  —  ($22  —  Zf$  12)^'  T  Lw  T  u  (4.4.22) 

where  the  eigenvalues  of  ($22  —  £$12)  can  be  assigned  arbitrarily  by  appropriate 
choice  of  the  3x3  matrix  L. 

Substituting  w  and  u  into  the  estimator  yields 

qx 2  =  (3>22  -  £$12)^2  +  L(qy  -  $112/  -  fin)  4-  ($21  y  +  f2)»  (4.4.23) 

but  this  equation  contains  qy  which  is  the  output  at  the  next  sampling  interval. 
The  term  qy  can  be  eliminated  if  we  define 

z  =  x 2-  Ly 
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with  the  observer 


qz  —  ($22  —  L$i2)(z  +  Ly)  +  ($21  ~  -£$11)2/  +  (r2  —  LY{)u 
—  ($22  —  -£$12)^  +  (f2  —  LTi)u 
+  [($22  —  Z$i2)Z  +  ($21  —  L$u)]y 


where 


x2  =  z  +  Ly 


is  the  estimate  of  x2  .  The  full  state  vector  is  then  given  by 


z(k  +  1)  =  ($22  -  L$12)z(k)  +  (f2  -  Zf  !)«(fc) 


+ 


($22  —  L$i2).L  +  ($21  -  T$n)  y(k) 


x(k)  = 


xi(k) 

x2(k) 


y{k) 

Ly(k)  +  z(k) 


4.4.3  Observer  Pole  Selection  and  Placement 


(4.4.24) 

(4.4.25) 


We  wish  to  design  the  observer  so  as  to  have  the  error  between  estimate  and 
actual  state  converge  asymptotically  to  zero,  so  we  design  to  observer  to  have 
faster  poles  than  the  highest  mode  frequency  of  the  closed  loop  system.  All 
poles  were  chosen  to  be  three  times  faster  than  the  highest  mode  frequency  of 
the  modeled  beam,  i.e. 

fa  -  3  x  20  =  60. 

If  the  poles  were  made  any  faster,  the  estimated  states  would  be  overly  sensitive 
to  any  sensor  noise  that  enters  into  the  observer  since  the  observer  also  should 
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have  a  desirable  noise  smoothing  effect  in  addition  to  estimating  the  states. 


4.4.4  An  Efficient  Observer  Algorithm 

An  algorithm  for  the  implementation  of  the  observer  achieves  an  increased  level 
of  efficiency  over  the  general  reduced  order  observer.  In  particular  the  update 
of  z(k)  in  equation  4.4.24  is  made  more  efficient  by  choosing  the  3x3  matrix  L 
such  that  the  matrix 

$22  —  .£/$  12  — 

Here  both  $22  and  $12  are  3x3  matrices  as  well  and  ts  is  the  sampling  period. 

This  diagonalizes  and  decouples  the  observer  dynamics  and  as  a  result  sim¬ 
plifies  the  computation  of  the  update  of  z(k).  Also  since  all  poles  are  identical, 
the  memory  space  requirements  are  reduced  although  this  is  not  really  an  issue 
in  this  particular  implementation  since  we  have  more  than  enough.  To  diagonal¬ 
ize  the  system,  we  must  solve  for  each  element  of  the  3x3  matrix  L  in  equation 
4.4.26. 

The  reason  we  can  achieve  this  diagonalization  is  because  we  have  three  sen¬ 
sors  and  thus  the  9  values  of  the  L  matrix  can  be  solved  for  the  9  particular 
values  that  we  want  in  the  matrix  4.4.26.  The  computation  of  L  which  places 
the  poles  and  diagonalizes  4.4.26  is  a  linear  set  of  9  equations  with  9  unknowns. 
Diagonalization  would  not  be  possible  if  we  had  less  than  three  sensor  measure- 


e-2*U,  Q 


0  e-2jr/<,«,  0 


Q  g-2  irf0t. 


(4.4.26) 
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merits  because  we  would  not  have  enough  parameters  in  the  L  matrix  to  choose 
such  that  the  elements  of  matrix  4.4.26  are  diagonal  in  addition  to  placing  the 
poles  of  the  observer. 

4.5  Console-Simnon  Optimization  of  Feedback  Gains 

Linear  state  feedback  of  the  form 

u(k)  =  kQ(vref  —  Kx{k))  .  (4.5.27) 

is  used  to  control  the  flexible  robot  where  Iv  is  a  6  x  1  feedback  gain  matrix 

k[  k-2  &3  k5  ke 

The  feedback  gains  are  optimized  using  Console  [14]  where  some  stringent  per¬ 
formance  criterion  is  placed  on  the  system  response.  The  criterion  specifies 
the  rise  time  and  overshoot  and  Console  optimizes  the  seven  feedback  gains  fc,-, 
i  =  0,  6,  to  best  achieve  the  performance  specifications. 

Unlike  the  work  of  Frank  [1]  and  Wang  [2],  an  additional  scale  factor  /c0  is 
also  optimized  to  scale  the  overall  error  from  the  reference  position.  Since  our 
motor  has  a  maximum  voltage  input  of  ±10  Volts,  corresponding  to  an  inter¬ 
nal  computer  representation  of  ±2048,  a  nonlinear  saturator  is  present  in  the 
system.  This  limits  our  ability  to  place  the  poles  of  the  system  arbitrarily  by  re¬ 
stricting  the  magnitude  of  the  control  to  the  motor.  The  additional  optimization 
parameter  /c0  allows  the  feedback  error  magnitude  to  be  so  large  in  magnitude 
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that  it  is  into  the  saturation  range.  The  optimization  is  performed  with  the 
saturator  present  and  this  ensures  that  the  controller  uses  all  of  the  available 
motor  torque.  In  the  thesis  of  both  Frank  and  Wang  the  reference  value  was 
fixed  and  the  magnitude  of  the  control  signal  was  well  under  the  limit  of  the 
saturator.  This  seems  to  suggest  that  we  can  significantly  increase  the  perfor¬ 
mance  by  scaling  up  ko  to  use  more  of  the  available  motor  torque  to  control  the 
flexible  manipulator. 

Another  benefit  of  having  a  larger  ko  is  that  ripple  torque  does  not  alter  the 
final  steady  state  position  as  much  by  forcing  the  shaft  position  toward  one  of 
the  stable  points  in  the  ripple  torque  cycle.  Under  final  steady  state  conditions, 
ripple  torque  present  in  the  system  tends  to  force  the  the  hub  into  the  location  of 
a  ’’potential  well”.  If  this  position  does  not  correspond  to  the  desired  reference 
position,  then  a  steady  state  positioning  error  will  be  caused  by  the  ripple  torque 
pulling  the  hub  away  from  the  desired  position.  With  an  increased  scale  factor 
kQ,  the  overall  gain  on  just  the  position  error  is  increased  and  this  will  cause  the 
final  position  accuracy  to  be  less  sensitive  to  the  ripple  torque. 

The  discretized  closed  loop  system  was  optimized  for  both  a  sampling  rate 
of  200  and  400  Hz  with  identical  criterions  on  performance  specifications.  The 
performance  specifications  are  defined  in  the  following  convert  file  in  figure  4.5. 

The  console  optimization  was  iterated  with  a  simple  PD  controller  as  the 
initial  reference  starting  gains  and  allowed  to  be  optimized  from  there.  The 
system  is  optimized  over  all  k{  and  the  final  optimal  set  of  feedback  gains  for 


74 


design__parameter  kl  init= 
design_parameter  k2  init= 
desigrL_parameter  k3  init= 
design_parameter  k4  init= 
design_parameter  k5  init= 
design_parameter  k6  init= 
design_parameter  kO  init= 


0  variational . 0e-4 
2 . 3  variations  .  3 
0.9  variations .  9 
0  variations .  0e- 3 
0  variations .  Oe-  3 
0  variations .  Oe-3 
3 . 0  variations . 0 


initialization  { 

simnon(  "syst  fbl2") ; 

simnon(  "store  v  yl  y2  y3  y4  y5  y6"  ) ; 

simu(  0.0,  1.6) ; 

> 


functional_ob j  ective  "overshoot" 
for  t  from  0  to  1.6  by  .005  - 
minimize  { 

double  output  ()  ; 
return  output (  "y4" ,  t) ; 

> 

good_curve  =  { 

if(  t  <=  0.65  )  return  1000; 
else'  return  1000; 

> 

bacLcurve  =  { 

if(  t  <=  0.65  )  return  1200; 
else  return  1100; 

> 

functional_obj ective  "rise" 

for  t  from  0.33  to  1.6  by  .005 
maximize  { 

double  output  ()  ; 
return  output (  "y4" ,  t) ; 

> 

good_curve  =  { 

if(  t  <=  0.4  )  return  900; 
else  return  999; 

> 

bad_curve  =  { 

if(  t  <=  0.5  )  return  600; 
else  return  900; 

> 

exit 


Figure  4.5:  Convert  File  of  Performance  Specifications 


Name 

Value 

Variation 

wrt  0 

Prev 

kl 

1.88801e-07 

1.0e-04 

*  *  *  *<y 

7% 

k2 

2 . 27634e+00 

2 . 3e+00 

0% 

k3 

1 . 05849e+00 

9.0e-01 

17% 

0% 

k4 

1.15239e-05 

1.0e-03 

****% 

0% 

k5 

6.66089e-06 

1.0e-03 

****% 

2% 

k6 

-1.18617e-05 

1.0e-03 

*  *  * 

0% 

kO 

3.45315e+00 

3 . 0e+00 

15% 

0% 

Pcomb  (Iter=  14)  (Phase  2)  (MAX_COST_SOFT=  0.0359949) 


SPECIFICATION  PRESENT  GOOD  G  B  BAD 

FOl  overshoot  1.01e+03  1.00e+03  - -  ■■==*  |  1.20e+03 

F02  rise  9.85e+02  9.99e+02  * . = - -  ■  = - — —  - — =  6.00e+02 

[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMMON]  [SIMNON]  [SIMMON]  [SIMNON] 

[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON] 

[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON] 

[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  Optimal  code  3 


<14>  cleaning  SIMNON  . . . 


Figure  4.6:  Optimal  Feedback  Gains  for  closed  loop  system  sampled  at  200  Hz 
the  two  sampling  rates  are  listed  in  figure  4.6  and  4.7. 

4.6  Sampling  Rate  Comparison 
4.6.1  200  Hertz 

The  step  response  of  the  experimental  system  with  the  feedback  observer  con¬ 
troller  in  place  is  shown  in  figures  4.8,  4.9,  and  4.10. 

Figure  4.10  shows  the  response  of  both  the  hub  position  and  the  tip  position 
which  is  generated  from  the  observer.  Although  this  plot  shows  some  noise,  it 
does  follow  the  expected  path  of  the  tip  and  the  nouminimurn  phase  oroperty 
is  clearly  present. 
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Name 

Value 

Variation 

vrt  0 

Prev 

kl 

-9.64316e-06 

1.0e-04 

****% 

0% 

k2 

2.39264e+00 

2.3e»00 

4% 

OX 

k3 

1 . 33221e+00 

9 . 0e-01 

48* 

ox 

k4 

-1.80057e-04 

1.0e-03 

ox 

kS 

1.74842e-04 

1.0e-03 

*•**% 

OX 

k6 

-1.01043e-04 

1.0e-03 

*»**% 

ox 

kO 

3.57706e+00 

3.0e+00 

19% 

ox 

Pcomb  (Iter=  20)  (Phase  2)  (MAX_COST_SOFT=  0.0449483) 

SPECIFICATION  PRESENT  GOOD  G  B  BAD 

FOl  overshoot  1.01e+03  1.00e+03  . . . *  |  1.20e+03 

F02  rise  9.81e+02  9.99e+02  « ■■  ■■■  .■  =x::— 6.00or02 

[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNQN]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON] 
[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON] 
[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON] 
(SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON] 
[SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON]  [SIMNON] 
[SIMNON]  [SIMNON]  [SIMNON]  Optimal  code  3 

<20>  cleaning  SIMNON  . . . 


Figure  4.7:  Optimal  Feedback  Gains  for  closed  loop  system  sampled  at  400  Hz 


Figure  4.8:  Accelerometer  step  response  of  physical  closed  loop  system.  200  Hz 


Figure  4.9:  Tachometer  step  response  of  physical  closed  loop  system:  200  Hz 


Figure  4.10:  Tip  and  Huh  position  step  response  of  physical  closed  loop  system: 
200  Hz 
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Figure  4.11:  Control  Signal  to  Motor:  200  Hz 
4.6.2  400  Hertz 

The  step  response  of  the  experimental  closed  loop  system  sampled  at  400  Hz 
with  the  feedback  observer  controller  in  place  is  shown  in  figures  4.8,  4.9,  and 
4.10. 

4.7  Performance  Comparison 

To  evaluate  the  performance  of  the  control  system  on  the  DSP32  chip,  a  step 
function  was  input  to  the  motor  to  study  how  the  manipulator  arm  moved  to 
its  new  position.  Figure  5  shows  the  motion  of  tiie  tip  position  as  a  function 
of  time.  The  tip  reaches  its  destination  approximately  0.5  second  later  witli  no 
overshoot.  Also  visible  from  the  graph  are  some  vibrations  due  to  the  flexibility 
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Motor  Control 


Figure  4.14:  Hub  position  step  response  of  physical  closed  loop  system:  400  Hz 


Figure  4.15:  Control  Signal  to  Motor:  400  Hz 
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of  the  beam  which  are  damped  out  as  the  tip  nears  its  destination.  Both  the 
200  Hz  and  the  400  Hz  have  nearly  identical  responses. 
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CHAPTER 

FIVE 


Digital  Signal  Processing  with  the  DSP32 


5.1  DSP32  Architecture 

Digital  signal  processing  applications  generally  require  a  large  number  of  repeti¬ 
tive  mathematical  operations  such  as  multiply  and  accumulate.  If  floating  point 
arithmetic  is  to  be  used,  conventional  microprocessors  and  co-processors,  which 
use  microcode  and  software  simulation  for  floating  point  arithmetic  operations, 
suffer  in  performance  compared  to  the  DSP  chips  which  have  floating  point  arith¬ 
metic  hardware.  The  AT&T  WE  DSP32  uses  a  fast  hardware  floating  point 
arithmetic  unit  which  can  peak  at  25  MFLOPS  in  the  latest  CMOS  version, 
the  DSP32C.  The  floating  point  ALU  is  highly  pipelined  and  can  achieve  two 
floating  point  operations  per  clock  cycle,  allowing  a  multiply  and  an  accumulate 
operation  execute  in  a  single  instruction  cycle. 

The  performance  and  costs  [15]  of  various  DSP  chips  summarized  in  Table 
5.1  is  adequate  for  many  real-time  applications  such  as  speech,  servo-control, 
and  graphics.  Previously,  dsp  chips  were  lacking  in  development  software,  but 
the  situation  is  improving  with  Optimizing  C  compilers  available  and  Software 
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Processor 

On-Chip 
Memory  (bits) 

Instruction 
Cycle  Time 

Mult./Accum. 

Time 

Single  Chip 
Cost 

AT&T  DSP32 

IK  x  32 

160  ns 

160  ns,  250ns 

$190 

AT&T  DSP32C 

IK  x  32 

80  ns 

80  ns  ,  100ns 

NA 

TI  TMS  320C1X 

256  x  16 

160  ns 

320  ns 

$50 

TI  TMS  32020 

288  x  16 

200  ns 

200  ns 

$120 

TI  TMS  320C30 

2K  x  32 

60  ns 

60  ns 

$1300 

AD  ADSP2100 

16  x  24 

125  ns 

80  ns  ,  100ns 

$411 

Table  5.1:  Comparison  of  Some  Commericially  Available  DSP  chips 


Libraries  of  common  arithmetic  functions.  The  DSP32  architecture  is  shown  in 
Figure  1  and  has  a  separate  address  bus  (16  bit)  and  data  bus  (32  bit),  Data 
Arithmetic  Unit  (DAU),  Control  Arithmetic  Unit  (CAU),  parallel  10  port,  and 
Serial  10  port,  and  four  banks  of  reconfigurable  memory. 


5.1.1  Data  Arithmetic  Unit 

The  DAU  is  the  main  data  processing  unit  for  DSP  algorithms  and  it  consists 
of  a  floating  point  multiplier,  an  adder,  and  four  static  40-bit  accumulators. 
The  floating  point  numbers  are  represented  in  a  special  32-bit  format  which  is 
compatible  with  the  DAU  hardware.  Inputs  to  the  multiplier  and  adder  can  be 
from  memory,  10,  or  another  accumulator.  The  multiplier  and  adder  operate  in 
parallel  and,  as  a  result,  there  are  certain  latency  effects  that  must  be  recognized 
when  performing  particular  software  tasks.  The  pipeline  achieves  the  single  cycle 
execution  for  all  instructions,  but  it  also  makes  control  of  the  processor  more 
difficult  and  this  emphasizes  the  need  for  a  separate  Control  Arithmetic  Unit 
(CAU)  on  the  DSP  chip. 
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Figure  5.1:  DSP32  Architecture  Block  Diagram,  source:  [17] 

5.1.2  Control  Arithmetic  Unit 

The  function  of  the  CAU  is  to  generate  the  addresses  to  memory  so  it  operates 
on  16  bit  integers  and  they  essentially  resemble  microprocessor  instructions.  The 
CAU  is  not  limited  to  generating  memory  addresses  exclusively  and  can  process 
integer  data  as  well  in  its  own  ALU.  There  are  twenty-one  16-bit  general  purpose 
registers  (rl-r21)  and  a  16-bit  program  counter.  Some  of  the  registers  have  spe¬ 
cial  functions  such  as  the  increment  registers  (rl5-rl9)  which  will  post  modify 
an  address  in  the  memory  pointers.  This  is  very  useful  in  signal  processing  algo¬ 
rithms  where  for  example,  the  tap  weights  of  a  filter  are  located  in  consecutive 
memory  locations  and  the  filter  can  just  step  through  the  weights  as  it  computes 
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the  output  of  the  filter.  The  DSP32  is  capable  of  Serial  DMA  and  the  pointer 
in  (r20)  and  pointer  out  (r21)  are  used  for  these  transfers. 

5.1.3  Memory 

The  memory  in  the  DSP32  may  be  accessed  as  8,  16,  or  32  bit  words.  In 
addition  to  the  single  cycle  instruction  execution,  up  to  four  memory  accesses 
per  instruction  cycle  are  possible;  instruction  fetch,  two  operand  fetches  and  a 
memory  write.  The  four  blocks  of  on-chip  memory  consist  of  one  block  with 
2Kbytes  ROM,  and  three  blocks  each  with  4  Kbytes  of  dynamic  RAM  (512  x  32 
words).  Memory  is  expandable  off  chip  up  to  a  maximum  of  56  Kbytes  which 
is  directly  accessible  by  addresses  generated  in  the  CAU.  The  four  blocks  and 
the  external  memory  can  be  configured  in  four  modes  as  shown  in  figure  2.  All 
of  the  modes  split  the  four  blocks  and  off-chip  memory  into  an  upper  and  lower 
bank.  The  reason  for  splitting  the  memory  into  two  is  for  the  DSP32  to  achieve 
maximum  throughput,  memory  accesses  must  alternate  between  the  two  banks 
and  the  different  modes  allow  us  to  specify  how  to  best  organize  the  memory 
for  our  application.  For  this  control  system  application  Memory  Mode  2  is  used 
since  it  maximizes  the  amount  of  total  memory  available.  In  this  pipelined 
architecture,  as  one  bank  is  being  accessed,  the  other  is  being  addressed  and 
the  pipelining  effectively  reduces  the  memory  access  time  by  one  half.  On  the 
other  hand,  it  is  possible  to  access  the  same  memory  bank  consecutively,  but  the 
control  unit  will  insert  a  wait  state  (  =  1/4  instruction  cycle)  and  thus  reduce 
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Figure  5.2:  Memory  Modes,  source:  [17] 
the  speed  of  the  computation. 

5.1.4  Processor  Cycle  and  Pipelining 

The  processor  cycle  for  the  multiply  and  accumulate  instruction 

*r 3  =  aO  =  al  +  *rl  *  *r2 

is  executed  in  four  states: 

1.  fetch  X  and  Y  fields  (  *rl  and  *r2  ) 

2.  multiply  *rl  *  *r 2 

3.  accumulate  product  with  al  and  put  result  in  aO 

4.  write  result  to  memory  (optional). 
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Because  of  the  pipelining,  we  get  single  cycle  execution  even  with  a  write 
to  memory.  This  is  useful  when  performing  such  tasks  as  windows,  adaptive 
filtering,  or  matrix  operations  that  can  take  a  block  of  data  form  memory,  process 
it  and  write  it  back  to  memory  all  in  one  instruction. 

5.1.5  Instruction  Set  and  Latency  Effects 

The  two  types  of  instructions  that  are  found  on  the  DSP32  are  the  Data  Arith¬ 
metic  (DA)  instructions  and  the  Control  Arithmetic  (CA)  instructions.  The  DA 
instructions  consist  of  multiply/accumulate  instructions  like 

[Z  =}aN  =  [ -]aM{+ ,  ~}Y  *  X 

and  some  special  functions  such  as:  float (),  int(),  round()  .  The  CA  instructions 
are  executed  in  the  CAU  and  consist  of  the  control,  arithmetic/logic,  and  data 
move  instructions.  Examples  of  these  are  respectively: 

•  if  (DA  COND)  goto  r3 

•  r2  =  r5  +  r6 

•  r3  =  memory-location 

There  are  three  classes  of  latencies  that  we  will  discuss  here: 

•  Memory  Writes 

•  Accumulator  as  Multiplier  Input 
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•  Conditional  Branching  on  DA  conditions 

When  a  DA  instruction  writes  to  memory,  the  value  written  is  not  available 
to  be  read  until  four  instructions  later. 

Example: 

1 1  *r3  =  aO  =  aO 

12  *r3  =  a3  =  a3 

12,  . 

14  . 

1 5  al  =  *r3 

Here  the  value  read  in  15  is  the  value  written  to  memory  in  II  not  12. 

When  an  accumulator,  a0-a3  is  used  as  an  input  to  the  multiplier,  its  value 
is  established  at  least  three  instructions  prior  to  the  multiply  instruction. 

Example: 

11  aO  =  aO  +  *rl  *  *r2 

12  aO  =  aO  +  al 

12  . 

/4  a2  =  aO  *  aO 

The  value  of  aO  used  in  14  is  computed  in  II  not  12. 

A  DAU  condition  tested  in  a  conditional  branch  is  established  by  the  DA 
instruction  which  is  at  least  four  instructions  before  the  test.  Example: 
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II  aO  —  aO  +  al 


12  a2  =  aO  *  a2 

13  . 

14  . 

14  if  (agt)  goto  next 

The  condition  ”agt”,  which  is  test  if  accumulator  is  greater  than  zero,  is 
established  by  II. 

5.2  Implementation  of  Observer-Controller  on  DSP32 

A  linear  control  system  for  the  flexible  link  manipulator  was  implemented  on 
the  DSP32  with  a  sampling  rate  of  200  Hz  and  400  Hz.  There  are  two  separate 
programs,  one  for  the  IBM  PC  and  one  for  the  DSP32.  The  IBM  PC  program 
is  written  in  C  and  compiled  using  the  Microsoft  C  compiler. 

The  C  program  initializes  the  analog  to  digital  converters,  initializes  the 
DSP32  and  downloads  the  DSP32  executable  file  to  the  memory  of  the  DSP 
board.  The  IBM  program  also  handles  the  calls  to  retrieve  input  from  sen¬ 
sors  and  to  send  out  a  new  control  signal  to  the  motor.  It  makes  use  of  some 
key  subroutines  supplied  by  Communications,  Automation,  and  Control  [16]  for 
initialization,  downloading  programs  and  data  and  uploading  results. 

The  program  for  the  DSP32,  written  in  assembly  language  and  assembled 
with  the  assembler  provided  by  AT&T,  is  where  the  algorithm  (see  figure  5.3 
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)  for  the  feedback  control  system  resides.  First,  the  program  waits  in  a  loop 
until  the  IBM  PC  downloads  the  new  sensor  samples  and  imediately  computes 
the  new  control  output  to  the  motor.  Then  the  DSP  algorithm  signals  the  IBM 
PC  that  the  motor  control  is  ready  (located  in  a  known  memory  location  on 
the  DSP32  board)  and  to  upload  the  value.  After  finishing  the  computation 
of  the  new  motor  control,  it  imediately  starts  updating  the  three  states  using 
the  reduced  order  state  observer.  This  architecture/algorithm  achieves  some 
degree  of  parallelism  here  since  the  DSP  chip  does  not  wait  for  the  PC  to  finish 
uploading  the  results  before  it  can  start  the  state  observer.  After  the  observer 
computation  computes  the  states  necessary  for  the  next  sampling  instant,  the 
algorithm  loops  back  and  waits  in  a  ’’wait  loop”  for  the  new  sensor  values  from 
the  PC  and  the  signal  to  start. 

The  signal  to  start  is  given  by  downloading  a  1  to  the  memory  location  ’’flag” 
and  the  signal  to  the  IBM  PC  that  results  are  ready  is  that  ’’flag”  is  set  to  0. 
The  IBM  program  constantly  monitors  this  memory  location  ’’flag”  to  determine 
when  the  control  is  ready  for  uploading. 

The  reduced  order  observer  calculation  is  split  into  two  parts  which  enables 
the  motor  control  to  be  output  virtually  imeadatelv  upon  receiving  the  latest 
sensor  data.  Only  that  part  of  the  observer  which  uses  the  latest  sensors  is 
used  placed  before  the  control  output.  The  part  of  the  observer  which  uses  the 
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Figure  5.3:  Flowchart  of  IBM  PC  program  synchronized  with  DSP32  Program 
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previous  sensor  values  ,  i.e. 


z(k  +  1)  =  Az{k)  +  bu(k)  +  Cy{k )  (5.2.1) 

was  done  at  the  end  of  the  previous  loop  while  the  control  was  being  uploaded 
and  is  stored  until  needed.  At  the  start  of  the  next  loop,  z(k  +  1)  is  used  in  the 
calculation 

{k  +  1)  =  Ly(k  +  1)  4-  z(k  +  1)  (5.2.2) 

and  then  together  with  the  three  sensors,  we  have  the  whole  state  vector  for 
feedback.  The  feedback  is  computed,  sent  through  the  saturator,  and  then 
offset  by  the  motor  offset  value  2005  before  it  is  uploaded  to  the  IBM  and  sent 
to  the  motor.  The  time  starting  from  the  point  where  the  DSP  chip  receives  the 
new  sensors  to  when  the  DSP  chip  sends  the  control  to  the  IBM  is  11  x  10~G 
seconds,  while  the  total  sampling  rate  is  either  5  X  10-3  or  2.5  x  1CT3  so  the 
control  is  output  almost  instaneously  once  the  new  sensors  are  in. 

A  time  saving  assumption  is  made  in  the  syncronization  scheme  and  that  is 
that  after  the  new  sensor  values  are  downloaded  to  the  DSP  board  and  the  flag  is 
set  to  1,  the  DSP  board  is  assumed  to  be  finished  with  the  observer  calculation. 
The  assumption  reduced  the  time  of  the  algorithm  since  the  C  program  does 
not  have  to  upload  a  flag  to  check  this  condition.  We  can  make  this  assumption 
since  the  computational  speed  of  the  DSP  board  is  so  greast  that.  it.  finishes  one 
loop  of  its  algorithm  much  faster  than  the  inter-sample  time. 
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I/O  Task 

Rate 

Upload  Rate 
Download  Rate 

0.728  Mbytes/sec 
0.412  Mbytes/sec 

Table  5.2:  Data  Transfer  Rates 


DSP  Task 

Time  Estimate 

Download  3  sensors  (6  bytes) 
Upload  motor  control  (2  bytes) 
Download  Flag  (2  bytes) 

Upload  Flag  (2  bytes) 

Linear  Feedback  Computation 
Observer  Computation 

Total  Time  Estimate 

14-lyu  sec 

2.7 fj,  sec 

4.7/isec 

2.7^sec 

93  x  250  nsec  =  23.25/i  sec 
45  x  250  nsec  =  11.25 jj.  sec 
59.15  n  sec 

Table  5.3:  Time  Estimates  of  DSP  Algorithmic  Processes 
5.2.1  Time  Estimates  of  DSP  Tasks 


The  data  transfer  rates  were  measured  between  the  DSP  board  memory  and  the 
IBM  PC  memory  and  the  estimates  are  tabulated  in  Table  5.2 

With  these  data  transfer  rates  and  the  number  of  operations  in  the  algo¬ 
rithm,  we  can  estimate  the  time  for  tasks  with  the  assumption  that  we  have  one 
operation  per  clock  cycle  with  a  clock  cycle  of  250  nanoseconds.  Table  5.3  gives 
the  estimates  of  individual  tasks  as  well  as  the  total  time  estimate.  The  time 
estimate  is  felt  to  be  a  lower  bound  on  the  total  time  since  wait  states  occai- 
sionaly  are  placed  to  get  required  values  out  of  the  pipeline  before  they  can  be 
used  for  a  subsequent  calculation.  We  therefore  need  to  allow  some  extra  time 
as  a  margin  of  safety  to  ensure  that  the  two  processors  remain  synchronised. 
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CHAPTER 

SIX 

Conclusion 


This  thesis  demonstrates  that  successful  control  system  design  is  achievable 
provided  we  have  the  proper  tools  at  hand.  To  summarize  what  we  have  designed 
and  implemented,  we  have: 

•  Increased  the  processing  power  of  the  digital  controller  through  use  of  a 
DSP  Chip. 

•  Achieved  some  computational  improvements  by  implementing  a  diagonal¬ 
ized  reduced  order  observer  which  takes  advantage  of  the  availability  of  all 
three  sensor  outputs. 

•  Significantly  improved  the  sampling  rate  to  adequately  measure  the  19Hz 
mode  and  allow  the  system  to  respond  to  disturbances  quicker. 

•  Implemented  an  observer  feedback  matrix  of  unrestricted  dimension,  i.e. 
L  is  a  3  x  3  matrix  with  all  nine  elements  available  for  pole  placement  in 
the  reduced  order  model.  In  previous  experiments  [1]  [2],  which  used  a 
full  sixth  order  observer,  the  observer  feedback  matrix  L  was  restrained 
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by  the  condition 


l  =  Zr  (6.0.1) 

where  L  is  a  6  x  3  matrix,  Z  is  1  x  3,  and  T  is  fixed  to  be  the  6x1  vector- 
defined  by  the  open  loop  system 

x(k  + 1)  =  $x(k)-\-Tu  (6.0.2) 

y{k)  =  Cx(k)  (6.0.3) 

Thus  only  three  parameters  were  available  to  the  control  system  designer 
in  constructing  the  observer. 

•  Optimized  the  feedback  gains  K  to  allow  an  overall  scaling  which  lets 
the  controller  make  full  use  of  all  available  motor  torque.  Although  the 
motor  saturator  causes  the  torque  to  occasionally  be  clipped,  the  overall 
system  step  response  is  approximately  three  times  faster  than  previous 
experiments  by  [2]  and  the  saturator  does  not  seem  to  cause  any  undesir¬ 
able  effects  in  the  system.  Since  the  system  in  optimized  using  Console 
with  the  saturator  in  place  ,  the  final  optimal  control  is  such  that  this 
nonlinearity  is  accounted  for  and  the  feedback  is  found  which  optimizes 
performance  under  the  control  magnitude  constraint.  With  a  large  slew 
angle  (  greater  or  equal  to  one  radian),  the  signal  to  the  motor  is  occasion¬ 
ally  clipped  since  the  controller  sends  to  the  motor  the  maximum  motor 
torque  allowed. 
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•  Performed  the  system  identification  with  the  friction  compensator  operat¬ 
ing  to  achieve  a  more  accurate  model.  As  a  test,  the  system  identification 
was  performed  without  the  friction  compensator  installed  and  this  system 
was  highly  damped  and  exhibited  minimal  vibrations  in  the  beam. 

In  addition  to  these  steps  which  let  to  the  successful  implementation,  a  use¬ 
ful  architecture  for  a  feedback  control  system  was  described  and  examples  of 
programming  the  processor  are  given  in  the  appendix.  The  Digital  Signal  Pro¬ 
cessor  seems  to  be  very  well  suited  for  linear  feedback  control  systems  and  if  any 
changes  were  to  be  made  in  the  future,  I  would  suggest  a  more  direct  method  of 
input  for  the  sensors  into  the  DSP  chip.  Timing  estimates  of  the  tasks  for  control 
are  measured  or  estimated  ,  as  the  case  may  be,  and  this  allows  us  to  determine 
an  upper  bound  on  the  sampling  rate  for  the  algorithm  we  have  implemented 
on  this  hardware. 

The  control  system,  as  we  have  designed  it,  has  undergone  a  state  variable 
transformation  which  makes  as  one  of  the'  states  (.t4)  correspond  to  the  tip 
position.  This  information  on  tip  position  is  not  available  directly  from  a  sensor 
and  we  must  settle  for  an  estimated  value.  The  tip  position  is  available  for 
feedback  along  with  the  other  states  to  achieve  the  performance  we  obtained. 
Console  proved  to  be  an  excellent  tool  for  successful  optimization  based  control 
system  design,  and  gives  some  insight  to  the  optimal  control.  The  feedback  from 
the  tip  position  state  for  the  optimized  system  was  small  suggesting  that  the 
controller  doesn’t  find  this  state  extremely  useful  in  controlling  the  tip.  Although 
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this  seems  paradoxical,  perhaps  it  can  be  explained  by  imagining  a  system  based 
on  tip  position  feedback  alone.  Such  a  system  has  some  instabilities  due  to  the 
nonminimum  phase  property  which  can  result  in  the  tip  being  ISO  degrees  out 
of  phase  with  the  hub  position  [3] . 

Furthermore,  an  analysis  of  the  nonlinear  Galerkin  model  was  undertaken 
and  through  simulation,  we  can  compare  this  ’’inextensible”  model  to  that  of  the 
linear  model  than  we  ultimately  implemented.  In  addition  to  this  comparison, 
the  nonminimum  phase  property  of  the  flexible  beam  is  clearly  visible  from  the 
impulse  response  of  the  3  body  Galerkin  model.  Such  analysis  and  simulation  is 
useful  for  successful  control  system  design  by  bringing  to  light  addition  insight 
to  beams  with  either  more  flexibility  or  quicker  slew  rates  which  will  require  an 
inextensible  Galerkin  model  from  which  to  design  the  control  system. 

Future  implementations  of  DSP  based  controllers  should  be  able  to  achieve 
over  lKHz  without  much  difficulty.  Even  now  current  DSP  processors  are  achiev¬ 
ing  better  than  25  MFLOPS  while  the  one  used  in  this  experiment  has  only  8 
MFLOPS.  While  the  speed  for  this  feedback  controller  processor  is  definitely 
I/O  bound,  we  can  still  achieve  excellent  overall  performance,  and  future  ar¬ 
chitectures  with  better  I/O  rates  will  significantly  improve  the  level  of  digital 
feedback  conti'ol  that  is  achievable. 
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Appendix 

A 


IBM  PC  AT  Program  to  Control  the  Flexible  Structure 


I  *************************************************************  j 

/*  Program.  control. c  */ 

/  *  DSP32  controller  */ 

j ************************************************************* j 

^include  <process.h> 

#include  <conio.h> 

#include  "dsp_util.c" 

extern  int  dashl6(); 
extern  int  dda6(); 
extern  void  exit(); 

int  mode,  basedl6=784,  dl6io[5],  flag; 
int  basedda6=768,  pa,  pb,  dda6ofst=2005; 
int  three=3,  eleven=ll,  four=4,  twelve=12; 

main() 

{ 

FILE  *fpl,*fp2,  *fp3,  *fp4,  *fp5,  *fp6; 

float  sensoff{3]; 
float  pos,  initpos; 
float  offset(); 

float  velplot[800],  accelplot[800]; 

float  tempf, scale; 

int  posplot[800]; 

int  control[800]; 

int  motor[800]; 

int  timing[800j; 

int  upsensor[3]; 

int  upl [800]; 

int  up2[800]; 

int  up3[800]; 

int  tippos[800]; 

int  sensor[3],  cflag; 

int  motjnp,  noloop; 

int  i,  j; 

int  loopcount  =9500; 
int  loopstop  =  9000; 
int  temp,pos_ref; 

unsigned  int  rcfad,  motad,  flagad,  controlad,  sensorad,  offad; 
unsigned  int  x2ad,  yad; 


/*****  Initialization  of  dashl 6  *****/ 

initdlG(); 


10 


20 


main 


30 


•10 


99 


motjnp  =  dda6ofst; 
dda6(  motjnp  ); 


main(cntrLc) 


outp(  basedda6+15,  Oxbe  ); 

!*****  Open  Files  *****/ 

if(  (fpl  =  fopen(  "encoder .plo",  'V  ))  ==  NULL  )  { 

fprintf(  stderr,  "couldn’t  open  file  count .plo\n"  );  60 

exit(  1  ); 

} 

if(  (fp2  =  fopen(  "timing. plo",  "w"  ))  ==  NULL  )  { 

fprintf(  stderr,  "couldn’t  open  file  count ,plo\n"  ); 
exit(  1  ); 

} 

if(  (fp3  =  fopen(  "accel.plo",  "w"  ))  ==  NULL  )  { 

fprintf(  stderr,  "couldn’t  open  file  control. plo\n"  );  70 

exit(  1  ); 

} 

if(  (fp4  =  fopen(  "control .plo",  "w"  ))  ==  NULL  )  { 

fprintf(  stderr,  "couldn’t  open  file  itgl2st.plo\n"  ); 
exit(  1  ); 

} 

if(  (fp5  =  fopen(  "tach.plo",  "w"  ))  ==  NULL  )  { 

fprintf(  stderr,  "couldn’t  open  file  itgl2.plo\n"  ); 
exit(  1  );  so 

} 

if(  (fp6  =  fopen(  "sensors .plo",  "w"  ))  ==  NULL  )  { 

fprintf(  stderr,  "couldn't  open  file  itgl2st ,plo\n"  ); 
exit(  1  ); 

} 

j *****  Compute  the  offset  value  for  velocity  and  acceleration  *****/ 
sensoff[l]  =  offset(  3,  —1.0  );  /*  Tachometer  */ 

sensoff[0]  =  ofFset(  4,  1.0  );  f*  Accelerometer  */  90 

printf("tach  offset:  V.i  accel  offset:  */.f\n",  sensofF[l],sensoff[0]); 

/*****  Start  Execution  *****/ 

printf("  Start  execution\n"); 
noloop  =  599; 

/**  Input  initial  position  data  ***/ 
pa  =  inp(  basedda6+12  ); 
pb  =  inp(  basedda6+13  ); 

initpos  =  ((pb*256  4-  pa)  >>  4)*3. 141592653/2048;  100 

printf("  Current  position  is  *Xf\n",  initpos  ); 

printf("  Input  new  position  (  in  radian  <  2)  "); 

scanf("7.f",  &pos  ); 

if(pos  >  2.0  )  pos=2.0; 
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main(cntrl.c) 


printf("  Input  scale  factor  "); 

scanf('"/,f ",  &scale); 

pos_ref  =  (int)  (pos*scale*1000.0); 

printf("pos_ref  =  ‘/.d\n“,pos_ref); 

default_addr(); 

if(!dsp_dl_exec("cntrl"))  no 

exit(l); 
dsp_run(); 

refad  =  get_addr("posref"); 
motad  =  get_addr("motor"); 
flagad  =  get_addr("dsp_flag"); 
sensorad  =  get_addr("sensor"); 
olfad  =  get_addr("off  set"); 
yad  =  get_addr("yi"); 
x2ad  =  get_addr("x2i"); 

printf("code  is  downloaded  to  DSP  and  its  running\n");  120 

/*Set  Multiplexer  Scan  limits  */ 
mode  =  one; 

dl6io[0]  =  three;  /*  Tachometer  channel  three  */ 

dl6io[l]  =  four;  /*  Accelerometer  channel  four  */ 

dashl6(  &mode,  dl6io,  &flag); 

/*****  Start  the  loop  *****/ 

dsp_dl_int(refad,  pos_ref);  130 

/*  Download  Sensor  Offsets  */ 

dsp_dl_array(offad,  4,  sensoff);  /*  f  since  floats  */ 

cflag=dsp_up_int(flagad) ; 

printf("cflag=Xd  (should  be  0)\n",cflag); 

dsp_dl_int(  flagad,  1  ); 

cflag=dsp_up_int(flagad); 

printf("cflag=‘/(d  (should  be  i)\n",cflag); 

for(  j  =  0;  j  <  noloop;  j++  )  {  140 

/*  Load  Counter  0  =  loopcount  */ 
mode  =  eleven; 
dl6io[0]  =  loopcount; 
dashl6(  fcmode,  dl6io  ,  &flag  ); 

j  *****  Input  position  data  *****/ 
pa  =  inp(  basedda6+12  ); 
pb  =  inp(  basedda6+13  ); 

sensor[2]  =  ((pb*256+pa)  »  4)*3. 141592653/2.048;  iso 

posplot[j]  =  sensor[2]; 

J*****  Input  velocity  and  acceleration  *****/ 
mode  =  three; 

dashl6(  &mode,  dl6io,  &flag  ); 
sensor[l]  =  dl6io[0] ; 
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dashl6(  &mode,  dl6io,  &flag); 
sensorjO]  =  dl6io[0]; 


main-get_addr(cntrl.c) 


dsp_dl_array(sensorad,  3,  sensor);  160 

dsp_dl_int  (flagad  ,1); 
cflag=dsp_up_int(flagad); 
while(0!=(cflag=dsp_up_int(flagad))) 
printf("y,d  '/.dW'jjCflag); 
motjnp  =  dsp_up_int(motad); 
dsp_up_array  (yad ,  3  ,upsensor) ; 
tipposfj]  =  dsp_up_int(x2ad); 
dda6(  motjnp  ); 
control])]  =  motjnp; 

velplotjj]  =  —  sensor[l]  —  sensofffl];  170 

accelplotjj]  =  sensor[0]  —  sensoffjO]; 

upl[j]  =  upsensor[0]; 

up2[j]  =  upsensor[l]; 

up3(j]  =  upsensor[2]; 

/*  DSP  is  now  computing  observer  */ 

/*  Use  up  remaining  time  before  taking  next  samples  *j 

mode  =  twelve; 

dl6io[0]  =  one;  /*  Latch  before  read  */  iso 

dashl6(  &mode,  dl6io,  &flag  ); 

timingp]=dl6io[l]; 

while(  loopstop  <=  dl6io[l]  )dashl6(  &mode,  dl6io,  &flag); 
dsp_halt(); 

/***  Input  final  position  data  ***/ 
pa  =  inp(  basedda6+12  ); 
pb  =  inp(  basedda6+13  ); 

initpos  =  ((pb*256  +  pa)  »  4)*3. 141592653/2048; 

printf("  Final  position  is  '/,f\n",  initpos  );  190 


/*****  The  End  *****/ 
dda6(  2005  ); 
printf("End  of  loop\n"); 
for(  j  =  0;  j  <  noloop;  j++  )  { 

fprintf(  fpl,",/.d,'/,d\n",  j,  posplot[j]); 
fprintf(  fp2,"y,d,'/.d\n"  j,  timingjj]); 
fprintf(  fp3,M,/.d,y,f\n",  j,  accelplot[j]); 
fprintf(  fp4,"%d,'/,d\n",  j,  2005  —  control])]); 

fprintf(  fp5,"y.d,y.f\n",  j,  velplotjj]);  200 

fprintf(  fp6,"*/.d  y,d  ’/,d  */.d  '/.d\n"  j,upl[j],up2[j],up3[j],tippos[j]); 

} 

printf("The  End.\n"); 

} 

get_addr(s)  get_addr 

char  *s; 

{ 
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get_addr(cntrl.c) 


unsigned  int  i; 

if  ((i=find_label_name(s))  ==  — 1){ 

printf("\nGlobal  label  ’'/.s’  missing  from  DSP32  program" ,s); 
exit(l); 

} 

return  i; 
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Appendix 

_ B 

DSP32  Program  to  Control  the  Flexible  Structure 

/*****************************************************  *******/ 

/**  cntrl.s  by  John  Bartusek  200  Hz  Sampling  Rate  **/ 

/**  DSP32  assembly  language  program  to  control  **/ 

/**  Flexible  Beam  Manipulator  **/ 

/************************************************************/ 
y************************************************************/ 

.rsect  ".bankO" 

.global  dsp_flag,  sensor,  motor,  offset,  posref,  start 
.global  loopl,yi,x2i 
rl  =  0 

*dsp_flag  =  rl 
3*nop 

start:  rl  =  *dsp_flag 

nop 

if  (eq)  goto  start 
nop 

call  _dsP32  Crl4) 
nop 
int  2 

int  offset 
rl8  =  -8 
rl  =  0 

*dsp_flag  =  rl 
3*nop 

loopl:  rl  =  *dsp_flag 

nop 

if  (eq)  goto  loopl 
nop 

r5  =  sensor  /*  sensors  downloaded  as  integers  */ 
rlO  =  posref 

rll  =  flref  /*  floating  point  of  posref  */ 

/*  Convert  sensors  to  floating  point  */ 

aO  =  float(*r5++)  /*  accel  */ 
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al  =  float (*r5++) 
a2  =  float(*r5) 
*rll  =  a3  =  float(*rlO) 


/*  tach  */ 
/*  pos  */ 
/*  posref  */ 


r6 

r7 

r8 

r9 


y  /*  sensors  converted  to  floating  pt  & 

sensgain  /*  sensor  gains 

offset  /*  sensor  offsets 

1  /*  Observer  Feedback  Matrix  L 


scaled  */ 
*/ 

*/ 

*/ 


*r6++  =  aO  =  -  *r8++  +  aO  *  *r7++  /*  Sensor  gains  */ 
*r6++  =  al  =  -  *r8 —  +  al  *  *r7++  /*  and  Offsets  */ 
*r6++r!8  =  a2  =  a2  *  *r7  /*  used  for  ref  */ 


nop 


/*  Compute  x2  =  L.y(k)  +  z(k)  */ 
r3  =  z 
r6  =  y 

rl3  =  x2  /*  3  vector  in  observer  x2=L.y(k)+z(k)  */ 

r7  -  ymd 

r8  =  d 

rl2  =  u 

aO  =  *rl2 

3*nop 


*r7++  = 
*r7++  = 
*r7  = 


al  =  *r6++ 
a2  =  *r6++ 
a3  =  *r6  - 
nop 

r7=ymd 


-  aO  *  *r8++ 

-  aO  *  *r8++ 
aO  *  *r8 


a0  =  *r9++  *  *r7++ 
aO  =  aO  +  *r9++  *  *r7++ 
aO  =  aO  +  *r9++  *  *r7++rl8 


*rl3++  =a0  =  aO  +  *r3++ 
r7=ymd 


aO  =  *r9++  *  *r7++ 
aO  =  aO  +  *r9++  *  *r7++ 
aO  =  aO  +  *r9++  *  *r7++rl8 
*rl3++  =a0  =  aO  +  *r3++ 
r7=ymd 

aO  =  *r9++  *  *r7++ 
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aO 

=  aO 

+ 

*r9++  *  *r7++ 

aO 

o 

aJ 

II 

+ 

*r9  *  *r7++rl8 

*rl3++rl8=a0 

=  aO 

+ 

*r3++rl8 

/*  Compute  Feedback 

control  law:  K 

r4 

=  k 

r6 

=  y 

rl3  =  x2 

aO 

= 

*r6++  *  *r4++ 

aO 

o 

II 

+ 

*r6++  *  *r4++ 

aO 

=  a0 

+ 

*r6++rl8  *  *r4++ 

aO 

o 

II 

+ 

*rl3++  *  *r4++ 

aO 

o 

rd 

II 

+ 

*rl3++  *  *r4++ 

aO 

=  a0 

+ 

*rl3++rl8  *  *r4 

/*  Subtract  from  reference  value  v  *  4000  -  K.x  */ 
r9  =  flref 

a3  =  -  aO  +  *r9  /*  ref  -  control  */ 

/*  Saturator  */ 

rl  =  satur 
r2  =  u 

a2  =  -*rl 
al  =  -a3  +  *rl 
a3  =  ifalt(*rl) 
al  =  a3  +  *rl 
*r2  =  a3  =  ifalt(a2) 

3*nop 

/*  Motor  Offset  */ 

r9  =  tipscale 
rlO  =  motor 
rll  =  motofst 
rl2  =  y 
rl3  =  x2 
rl  =  yi 
r2  =  x2i 

al  =  -a3  +  *rll  /*  2005  -  u  */ 

a3  =  *ri3  *  *r9 
5*nop 


/*  initialize  a2  with  negative  limit  */ 
/*  compare  a3  with  positive  limit  */ 
/*  if  a3>2000,  replace  a3  with  2000  */ 
/*  compare  a3  with  negative  limit  */ 
/*  if  a3<-2000,  replace  a3  with  -2000  */ 
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*rlO  =  aO=  int(al) 

*rl++  =  al  =  int(*rl2++) 

*rl++  =  a2  =  int(*rl2++) 

*rl  =  aO  =  int(*rl2) 

*r2  =  aO  =  int(a3) 

4*nop 

/*  Send  out  Control  Signal  */ 

rl  =  0 

*dsp_flag  =  rl 

/*  Update  State  Vector  */ 

r2  =  a 
r3  =  z 
r4  =  b 
r5  =  c 
r6  =  y 
r7  =  ymd 
r8  =  d 
rl2  =  nextz 
rl3  =  u 
aO  =  *rl3 
3*nop 

*r7++  =  al  =  *r6++  -  aO  *  *r8++ 

*r7++  =  a2  =  *r6++  -  aO  *  *r8++ 

*r7  =  a3  =  *r6  -  aO  *  *r8 

nop 

r7=ymd 

aO  =  *r2  *  *r3++  /*  a  *  z  */ 

aO  =  aO  +  *r4++  *  *rl3  /*  +  b  *  u  */ 
aO  =  aO  +  *r5++  *  *r7++  /*  +  c  *  y  */ 
aO  =  aO  +  *r5++  *  *r7++ 

*rl2++  =a0  =  aO  +  *r5++  *  *r7++rl8 
nop 

r7=ymd 


aO  =  *r2  *  *r3++ 


aO  =  aO 

+ 

*r4++ 

* 

*rl3 

aO  =  aO 

+ 

*r5++ 

* 

*r7++ 

aO  =  aO 

+ 

*r5++ 

* 

*r7++ 

*rl2++  =a0  =  aO 

+ 

*r5++ 

* 

*r7++r!8 
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nop 

r7=ymd 

aO  =  *r2  *  *r3++rl8  /*  skip  back  8  */ 

aO  =  aO  +  *r4++  *  *r!3 

aO  =  aO  +  *r5++  *  *r7++ 

aO  =  aO  +  *r5++  *  *r7++ 

*rl2++rl8=a0  =  aO  +  *r5++  *  *r7++rl8 

nop 
r3=z 

r!2=nextz 


/*  Update  nextz  to  z  */ 


*r3++  =  aO  = 

*rl2++ 

*r3++  =  al  = 

*rl2++ 

*r3  =  a2  = 

goto  loopl 
nop 

*rl2 

posref : 

float  0.0 

offset : 

2*float  0.0 

motof st : 

float  2005.0 

u: 

float  0.0 

nextz: 

3*float  0.0 

satur : 

float  2000.0 

y; 

3*float  0.0 

z: 

3*float  0.0 

x2: 

3*float  0.0 

ymd: 

3*float  0.0 

f lref : 

float  0.0 

tipscale : 

float  0.091 

motor: 

int  0 

dsp_f lag: 

int  0 

sensor: 

3*int  0 

x2i : 

3*int  0 

yi: 

3*int  0 

.align  4 
/* 

_dsp32  (n,A) 
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const  int  n; 

float  *A; 

{ 

scratch  register  short  rl,  r2,  r3,  r5; 
scratch  pointer  register  short  r4,  r6,  rl4; 
scratch  increment  register  short  rl5,  rl6; 
scratch  register  float  aO; 

#asm 
*/ 

_dsp32: 

r5  =  *rl4++ 
r4  =  *rl4++ 
r5  =  r5  -  2 
r6  =  _dsp32T 
rl5  =  3 
rl6  =  -3 

rl  =  *r4++ 

_dsp32A:  r2  =  *r4 

*r4 —  =  rlh 
*r4 —  =  rll 
r3  =  r2*2 
*r4++rl5  =  r3h 
r2  =  r2  &  0x807f 
if (pi)  goto  _dsp32B 
*r4++rl6  =  r21  /*  b3  */ 

goto  _dsp32C 

*r4++  =  aO  =  -  *r4  *  *r6  /*  negaitve  number  */ 

nop  /*  Dummy  to  make  assembler  work  */ 

_dsp32B:  *r4++  =  aO  =  *r4  *  *r6  /*  positive  number  */ 

_dsp32C:  if(r5 —  >=0)  goto  _dsp32A 
rl  =  *r4++ 


/*  rS  =  Length  of  Array  */ 

/*  r4  points  to  start  of  Array  */ 
/*  r5  is  a  loop  counter  */ 

/*  r6  points  to  float  2.0  */ 

/*  pointer  bumper  */ 

/*  pointer  bumper  */ 

/*  wO  */ 

/*  wl  */ 

/*  b2  */ 

/*  bl  */ 

/*  bO,  i.e.,  exponent  field  */ 


nop 

_dsp32T:  float  2.0 
/* 

#endasm 

> 

*/ 

rsect  ".hi_ram" 
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a: 
b: 
c : 


kp: 

k: 

1: 


sensgain : 
d: 


float  0.1518358 

float  -3.0847574,-1.1355494,  -1.1145619 
float  9.798564,  -25.883911,  -177.5527 
float  3.6014428,  -9.766553,  -70.725845 
float  3.539703,-9.679377,  -77.52354 
float  4.28e-5,  11.0,  5.045 
float  -4.24e-4,  0.0,  0.0 
float  6 . 5195817e-7 , 6 . 860543471 , 3 . 45315 
float  3 . 9793755e-5 , 2 . 300105e-5 , -4 . 095845e-5 
float  -3.4019299,  20.70356,  220.12013 

float  -1.2499559,  5.564637,  84.12455 
float  -1.2383009,  5.3820543,  85.07537 
float  1.0,  -1.15,  1.0 
float  -0.036449,  4.40807e-8,  4.1436e-5 
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